Merge "Add DMAPulseSeparationReader for shooter tuning"
diff --git a/aos/events/logging/log_namer.cc b/aos/events/logging/log_namer.cc
index 1383aed..9a5d038 100644
--- a/aos/events/logging/log_namer.cc
+++ b/aos/events/logging/log_namer.cc
@@ -201,7 +201,7 @@
const UUID &source_node_boot_uuid = state[node_index].boot_uuid;
const Node *const source_node =
configuration::GetNode(configuration_, node_index);
- CHECK_EQ(LogFileHeader::MiniReflectTypeTable()->num_elems, 24u);
+ CHECK_EQ(LogFileHeader::MiniReflectTypeTable()->num_elems, 28u);
flatbuffers::FlatBufferBuilder fbb;
fbb.ForceDefaults(true);
@@ -258,41 +258,32 @@
std::vector<flatbuffers::Offset<flatbuffers::String>> boot_uuid_offsets;
boot_uuid_offsets.reserve(state.size());
- for (const NewDataWriter::State &state : state) {
- if (state.boot_uuid != UUID::Zero()) {
- boot_uuid_offsets.emplace_back(state.boot_uuid.PackString(&fbb));
- } else {
- boot_uuid_offsets.emplace_back(fbb.CreateString(""));
- }
- }
- flatbuffers::Offset<
- flatbuffers::Vector<flatbuffers::Offset<flatbuffers::String>>>
- boot_uuids_offset = fbb.CreateVector(boot_uuid_offsets);
-
- int64_t *oldest_remote_monotonic_timestamps;
+ int64_t *unused;
flatbuffers::Offset<flatbuffers::Vector<int64_t>>
oldest_remote_monotonic_timestamps_offset = fbb.CreateUninitializedVector(
- state.size(), &oldest_remote_monotonic_timestamps);
+ state.size(), &unused);
- int64_t *oldest_local_monotonic_timestamps;
flatbuffers::Offset<flatbuffers::Vector<int64_t>>
oldest_local_monotonic_timestamps_offset = fbb.CreateUninitializedVector(
- state.size(), &oldest_local_monotonic_timestamps);
+ state.size(), &unused);
- int64_t *oldest_remote_unreliable_monotonic_timestamps;
flatbuffers::Offset<flatbuffers::Vector<int64_t>>
oldest_remote_unreliable_monotonic_timestamps_offset =
fbb.CreateUninitializedVector(
- state.size(), &oldest_remote_unreliable_monotonic_timestamps);
+ state.size(), &unused);
- int64_t *oldest_local_unreliable_monotonic_timestamps;
flatbuffers::Offset<flatbuffers::Vector<int64_t>>
oldest_local_unreliable_monotonic_timestamps_offset =
fbb.CreateUninitializedVector(
- state.size(), &oldest_local_unreliable_monotonic_timestamps);
+ state.size(), &unused);
for (size_t i = 0; i < state.size(); ++i) {
+ if (state[i].boot_uuid != UUID::Zero()) {
+ boot_uuid_offsets.emplace_back(state[i].boot_uuid.PackString(&fbb));
+ } else {
+ boot_uuid_offsets.emplace_back(fbb.CreateString(""));
+ }
if (state[i].boot_uuid == UUID::Zero()) {
CHECK_EQ(state[i].oldest_remote_monotonic_timestamp,
monotonic_clock::max_time);
@@ -304,20 +295,32 @@
monotonic_clock::max_time);
}
- oldest_remote_monotonic_timestamps[i] =
- state[i].oldest_remote_monotonic_timestamp.time_since_epoch().count();
- oldest_local_monotonic_timestamps[i] =
- state[i].oldest_local_monotonic_timestamp.time_since_epoch().count();
- oldest_remote_unreliable_monotonic_timestamps[i] =
- state[i]
- .oldest_remote_unreliable_monotonic_timestamp.time_since_epoch()
- .count();
- oldest_local_unreliable_monotonic_timestamps[i] =
- state[i]
- .oldest_local_unreliable_monotonic_timestamp.time_since_epoch()
- .count();
+ flatbuffers::GetMutableTemporaryPointer(
+ fbb, oldest_remote_monotonic_timestamps_offset)
+ ->Mutate(i, state[i]
+ .oldest_remote_monotonic_timestamp.time_since_epoch()
+ .count());
+ flatbuffers::GetMutableTemporaryPointer(
+ fbb, oldest_local_monotonic_timestamps_offset)
+ ->Mutate(i, state[i]
+ .oldest_local_monotonic_timestamp.time_since_epoch()
+ .count());
+ flatbuffers::GetMutableTemporaryPointer(
+ fbb, oldest_remote_unreliable_monotonic_timestamps_offset)
+ ->Mutate(i, state[i]
+ .oldest_remote_unreliable_monotonic_timestamp.time_since_epoch()
+ .count());
+ flatbuffers::GetMutableTemporaryPointer(
+ fbb, oldest_local_unreliable_monotonic_timestamps_offset)
+ ->Mutate(i, state[i]
+ .oldest_local_unreliable_monotonic_timestamp.time_since_epoch()
+ .count());
}
+ flatbuffers::Offset<
+ flatbuffers::Vector<flatbuffers::Offset<flatbuffers::String>>>
+ boot_uuids_offset = fbb.CreateVector(boot_uuid_offsets);
+
aos::logger::LogFileHeader::Builder log_file_header_builder(fbb);
log_file_header_builder.add_name(name_offset);
diff --git a/aos/events/logging/logfile_sorting.cc b/aos/events/logging/logfile_sorting.cc
index 3f637f8..aeced9f 100644
--- a/aos/events/logging/logfile_sorting.cc
+++ b/aos/events/logging/logfile_sorting.cc
@@ -89,7 +89,7 @@
}
bool ConfigOnly(const LogFileHeader *header) {
- CHECK_EQ(LogFileHeader::MiniReflectTypeTable()->num_elems, 24u);
+ CHECK_EQ(LogFileHeader::MiniReflectTypeTable()->num_elems, 28u);
if (header->has_monotonic_start_time()) return false;
if (header->has_realtime_start_time()) return false;
if (header->has_max_out_of_order_duration()) return false;
@@ -1238,8 +1238,15 @@
return a.second < b.second;
});
new_parts.parts.reserve(parts.second.parts.size());
- for (std::pair<std::string, int> &p : parts.second.parts) {
- new_parts.parts.emplace_back(std::move(p.first));
+ {
+ int last_parts_index = -1;
+ for (std::pair<std::string, int> &p : parts.second.parts) {
+ CHECK_LT(last_parts_index, p.second)
+ << ": Found duplicate parts in '" << new_parts.parts.back()
+ << "' and '" << p.first << "'";
+ last_parts_index = p.second;
+ new_parts.parts.emplace_back(std::move(p.first));
+ }
}
CHECK(!parts.second.config_sha256.empty());
diff --git a/aos/events/logging/logger.fbs b/aos/events/logging/logger.fbs
index c71baad..6d9f7cb 100644
--- a/aos/events/logging/logger.fbs
+++ b/aos/events/logging/logger.fbs
@@ -136,13 +136,17 @@
// For all channels sent from a specific node, these vectors hold the
// timestamp of the oldest known message from that node, and the
// corresponding monotonic timestamp for when that was received on our node.
- oldest_remote_monotonic_timestamps:[int64] (id: 20);
- oldest_local_monotonic_timestamps:[int64] (id: 21);
+ corrupted_oldest_remote_monotonic_timestamps:[int64] (id: 20, deprecated);
+ corrupted_oldest_local_monotonic_timestamps:[int64] (id: 21, deprecated);
+ oldest_remote_monotonic_timestamps:[int64] (id: 24);
+ oldest_local_monotonic_timestamps:[int64] (id: 25);
// For all channels *excluding* the reliable channels (ttl == 0), record the
// same quantity.
- oldest_remote_unreliable_monotonic_timestamps:[int64] (id: 22);
- oldest_local_unreliable_monotonic_timestamps:[int64] (id: 23);
+ corrupted_oldest_remote_unreliable_monotonic_timestamps:[int64] (id: 22, deprecated);
+ corrupted_oldest_local_unreliable_monotonic_timestamps:[int64] (id: 23, deprecated);
+ oldest_remote_unreliable_monotonic_timestamps:[int64] (id: 26);
+ oldest_local_unreliable_monotonic_timestamps:[int64] (id: 27);
}
// Table holding a message.
diff --git a/aos/events/logging/logger_test.cc b/aos/events/logging/logger_test.cc
index 226c084..8d8a2a6 100644
--- a/aos/events/logging/logger_test.cc
+++ b/aos/events/logging/logger_test.cc
@@ -2501,6 +2501,30 @@
ConfirmReadable(pi1_single_direction_logfiles_);
}
+// Tests that we explode if someone passes in a part file twice with a better
+// error than an out of order error.
+TEST_P(MultinodeLoggerTest, DuplicateLogFiles) {
+ time_converter_.AddMonotonic(
+ {BootTimestamp::epoch(),
+ BootTimestamp::epoch() + chrono::seconds(1000)});
+ {
+ LoggerState pi1_logger = MakeLogger(pi1_);
+
+ event_loop_factory_.RunFor(chrono::milliseconds(95));
+
+ StartLogger(&pi1_logger);
+
+ event_loop_factory_.RunFor(chrono::milliseconds(10000));
+ }
+
+ std::vector<std::string> duplicates;
+ for (const std::string &f : pi1_single_direction_logfiles_) {
+ duplicates.emplace_back(f);
+ duplicates.emplace_back(f);
+ }
+ EXPECT_DEATH({ SortParts(duplicates); }, "Found duplicate parts in");
+}
+
// Tests that we properly handle a dead node. Do this by just disconnecting it
// and only using one nodes of logs.
TEST_P(MultinodeLoggerTest, DeadNode) {
diff --git a/debian/matplotlib.BUILD b/debian/matplotlib.BUILD
index 881521c..ad44398 100644
--- a/debian/matplotlib.BUILD
+++ b/debian/matplotlib.BUILD
@@ -2,10 +2,5 @@
build_matplotlib(
"3",
- tkinter_py_version = "3.5",
-)
-
-build_matplotlib(
- "2.7",
- copy_shared_files = False,
+ tkinter_py_version = "3.7",
)
diff --git a/debian/matplotlib.bzl b/debian/matplotlib.bzl
index d94efd9..3ae0786 100644
--- a/debian/matplotlib.bzl
+++ b/debian/matplotlib.bzl
@@ -185,11 +185,19 @@
name = "matplotlib" + version,
srcs = _src_copied + [
version + "/matplotlib/__init__.py",
- ],
+ ] + native.glob(
+ include = ["usr/lib/python" + tkinter_py_version + "/**/*.py"],
+ ),
data = _data_files + _builtin_so_copied + _system_so_copied + [
":usr/share/matplotlib/mpl-data/matplotlibrc",
- ] + native.glob(["etc/**"]),
- imports = ["usr/lib/python" + version + "/dist-packages", version, "."],
+ ] + native.glob(["etc/**", "usr/share/fonts/**"]),
+ imports = [
+ "rpathed3/usr/lib/python" + version + "/dist-packages",
+ "rpathed3/usr/lib/python" + version + ".7/lib-dynload",
+ version,
+ ".",
+ "usr/lib/python" + tkinter_py_version,
+ ],
target_compatible_with = ["@platforms//cpu:x86_64"],
visibility = ["//visibility:public"],
)
diff --git a/debian/matplotlib_init.patch b/debian/matplotlib_init.patch
index 4cc155a..2818cd6 100644
--- a/debian/matplotlib_init.patch
+++ b/debian/matplotlib_init.patch
@@ -26,7 +26,7 @@
+
+# Tell fontconfig where to find matplotlib's sandboxed font files.
+os.environ["FONTCONFIG_PATH"] = os.path.join(_matplotlib_base, "etc/fonts")
-+os.environ["FONTCONFIG_FILE"] = os.path.join(_matplotlib_base, "etc/fonts/fonts.conf")
++os.environ["FONTCONFIG_FILE"] = "fonts.conf"
+os.environ["FONTCONFIG_SYSROOT"] = _matplotlib_base
+
try:
diff --git a/frc971/analysis/BUILD b/frc971/analysis/BUILD
index 2a945bf..5457405 100644
--- a/frc971/analysis/BUILD
+++ b/frc971/analysis/BUILD
@@ -47,6 +47,7 @@
"//frc971/control_loops/drivetrain:robot_state_plotter",
"//frc971/control_loops/drivetrain:spline_plotter",
"//frc971/wpilib:imu_plotter",
+ "//y2020/control_loops/drivetrain:localizer_plotter",
"//y2020/control_loops/superstructure:accelerator_plotter",
"//y2020/control_loops/superstructure:finisher_plotter",
"//y2020/control_loops/superstructure:hood_plotter",
diff --git a/frc971/analysis/plot_index.ts b/frc971/analysis/plot_index.ts
index 8b8c951..7b46a70 100644
--- a/frc971/analysis/plot_index.ts
+++ b/frc971/analysis/plot_index.ts
@@ -32,6 +32,8 @@
'org_frc971/y2020/control_loops/superstructure/finisher_plotter'
import {plotTurret} from
'org_frc971/y2020/control_loops/superstructure/turret_plotter'
+import {plotLocalizer} from
+ 'org_frc971/y2020/control_loops/drivetrain/localizer_plotter'
import {plotAccelerator} from
'org_frc971/y2020/control_loops/superstructure/accelerator_plotter'
import {plotHood} from
@@ -100,6 +102,7 @@
['Accelerator', new PlotState(plotDiv, plotAccelerator)],
['Hood', new PlotState(plotDiv, plotHood)],
['Turret', new PlotState(plotDiv, plotTurret)],
+ ['2020 Localizer', new PlotState(plotDiv, plotLocalizer)],
['C++ Plotter', new PlotState(plotDiv, plotData)],
]);
diff --git a/frc971/control_loops/drivetrain/hybrid_ekf.h b/frc971/control_loops/drivetrain/hybrid_ekf.h
index 04ffa4e..ecdabb8 100644
--- a/frc971/control_loops/drivetrain/hybrid_ekf.h
+++ b/frc971/control_loops/drivetrain/hybrid_ekf.h
@@ -137,7 +137,7 @@
};
static constexpr int kNInputs = 4;
// Number of previous samples to save.
- static constexpr int kSaveSamples = 50;
+ static constexpr int kSaveSamples = 80;
// Whether we should completely rerun the entire stored history of
// kSaveSamples on every correction. Enabling this will increase overall CPU
// usage substantially; however, leaving it disabled makes it so that we are
diff --git a/frc971/control_loops/python/BUILD b/frc971/control_loops/python/BUILD
index b6290d4..a4b8fb8 100644
--- a/frc971/control_loops/python/BUILD
+++ b/frc971/control_loops/python/BUILD
@@ -112,6 +112,7 @@
":drivetrain",
":python_init",
"//external:python-glog",
+ "@matplotlib_repo//:matplotlib3",
],
)
diff --git a/frc971/control_loops/python/field_images/2020.png b/frc971/control_loops/python/field_images/2020.png
index 166a1bb..2397fa0 100644
--- a/frc971/control_loops/python/field_images/2020.png
+++ b/frc971/control_loops/python/field_images/2020.png
Binary files differ
diff --git a/frc971/control_loops/python/path_edit.py b/frc971/control_loops/python/path_edit.py
index d743e0d..2e4e269 100755
--- a/frc971/control_loops/python/path_edit.py
+++ b/frc971/control_loops/python/path_edit.py
@@ -444,6 +444,12 @@
# scale from point in field coordinates
point = self.mousex, self.mousey
+ # This restricts the amount it can be scaled.
+ if self.transform.xx <= 0.4:
+ scale = max(scale, 1)
+ elif self.transform.xx >= 4:
+ scale = min(scale, 1)
+
# move the origin to point
self.transform.translate(point[0], point[1])
diff --git a/y2020/BUILD b/y2020/BUILD
index 45164b4..19debdc 100644
--- a/y2020/BUILD
+++ b/y2020/BUILD
@@ -232,6 +232,7 @@
"//aos/network:timestamp_fbs",
"//y2020/control_loops/superstructure:superstructure_goal_fbs",
"//y2019/control_loops/drivetrain:target_selector_fbs",
+ "//y2020/control_loops/drivetrain:localizer_debug_fbs",
"//y2020/control_loops/superstructure:superstructure_output_fbs",
"//y2020/control_loops/superstructure:superstructure_position_fbs",
"//y2020/control_loops/superstructure:superstructure_status_fbs",
diff --git a/y2020/actors/splines/README.md b/y2020/actors/splines/README.md
new file mode 100644
index 0000000..058f5bc
--- /dev/null
+++ b/y2020/actors/splines/README.md
@@ -0,0 +1,24 @@
+# Spline Descriptions
+This folder contains reference material for what each spline does
+
+<br>
+
+# Target Aligned - Infinite Recharge
+## After shooting balls initially, collects power cells and returns to shooting position
+
+### [Target Aligned 1](target_aligned_1.json)
+After shooting the pre-loaded balls, this spline directs the robot to the power cells, allowing the robot to collect all three balls available
+
+### [Target Aligned 2](target_aligned_2.json)
+After collecting the 3 balls using the target_aligned_1 spline, this spline directs the robot back towards the shooter tower, setting it up to shoot the balls it collected
+
+<br>
+
+# Target Offset - Infinite Recharge
+## After starting with 3 balls, collects power cells and heads to primary shooting position
+
+### [Target Offset 1](target_offset_1.json)
+When we start offset from the target with the 3 pre-loaded balls in the robot, this spline directs the robot to the additional 2 power cells, allowing the robot to collect both balls available
+
+### [Target Offset 2](target_offset_2.json)
+After collecting the 2 balls using the target_offset_1 spline, this spline directs the robot towards the shooter tower, setting it up to shoot the 5 balls.
\ No newline at end of file
diff --git a/y2020/actors/splines/target_aligned_1.json b/y2020/actors/splines/target_aligned_1.json
new file mode 100644
index 0000000..5a1d082
--- /dev/null
+++ b/y2020/actors/splines/target_aligned_1.json
@@ -0,0 +1 @@
+{"spline_count": 1, "spline_x": [3.340495531674842, 3.3229239110760016, 4.483964997108113, 6.058132772881773, 7.084092118999326, 8.012654807002901], "spline_y": [5.730681749413974, 6.367337321705237, 7.14109740201818, 7.627912651837696, 7.449284138182865, 7.4135680036085025], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3.0}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2.0}, {"constraint_type": "VOLTAGE", "value": 10.0}]}
\ No newline at end of file
diff --git a/y2020/actors/splines/target_aligned_2.json b/y2020/actors/splines/target_aligned_2.json
new file mode 100644
index 0000000..bca8965
--- /dev/null
+++ b/y2020/actors/splines/target_aligned_2.json
@@ -0,0 +1 @@
+{"spline_count": 1, "spline_x": [8.02107576923077, 7.157915384615384, 6.042122692307692, 5.010540769230768, 4.2315911538461535, 3.368430769230769], "spline_y": [7.389495, 7.431600384615384, 6.652650769230769, 6.1473861538461545, 5.7684376923076925, 5.7473849999999995], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3.0}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2.0}, {"constraint_type": "VOLTAGE", "value": 10.0}]}
\ No newline at end of file
diff --git a/y2020/actors/splines/target_offset_1.json b/y2020/actors/splines/target_offset_1.json
new file mode 100644
index 0000000..7b56061
--- /dev/null
+++ b/y2020/actors/splines/target_offset_1.json
@@ -0,0 +1 @@
+{"spline_count": 1, "spline_x": [3.31276940587483, 3.2907022466964535, 4.149941560222061, 3.9732949238578668, 4.6679269035533, 5.751552791878172], "spline_y": [2.721120947225566, 1.4302342949805793, 1.6475505418770833, 1.4726197969543149, 0.8891289340101522, 0.8335583756345177], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3.0}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2.0}, {"constraint_type": "VOLTAGE", "value": 10.0}]}
\ No newline at end of file
diff --git a/y2020/actors/splines/target_offset_2.json b/y2020/actors/splines/target_offset_2.json
new file mode 100644
index 0000000..6292a7f
--- /dev/null
+++ b/y2020/actors/splines/target_offset_2.json
@@ -0,0 +1 @@
+{"spline_count": 1, "spline_x": [5.709874873096447, 4.5984637055837565, 4.112221319796954, 3.8482611675126903, 3.7232274111675125, 3.3481261421319797], "spline_y": [0.8057730964467005, 0.7779878172588832, 1.4170492385786801, 2.875776395939086, 4.167791878172588, 5.862693908629441], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3.0}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2.0}, {"constraint_type": "VOLTAGE", "value": 10.0}]}
\ No newline at end of file
diff --git a/y2020/control_loops/drivetrain/BUILD b/y2020/control_loops/drivetrain/BUILD
index 89d8044..7c4f99f 100644
--- a/y2020/control_loops/drivetrain/BUILD
+++ b/y2020/control_loops/drivetrain/BUILD
@@ -1,6 +1,7 @@
load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
load("//aos:config.bzl", "aos_config")
load("//tools/build_rules:select.bzl", "compiler_select", "cpu_select")
+load("@npm_bazel_typescript//:defs.bzl", "ts_library")
genrule(
name = "genrule_drivetrain",
@@ -56,13 +57,22 @@
],
)
+flatbuffer_cc_library(
+ name = "localizer_debug_fbs",
+ srcs = ["localizer_debug.fbs"],
+ gen_reflections = 1,
+ visibility = ["//visibility:public"],
+)
+
cc_library(
name = "localizer",
srcs = ["localizer.cc"],
hdrs = ["localizer.h"],
target_compatible_with = ["@platforms//os:linux"],
deps = [
+ ":localizer_debug_fbs",
"//aos/containers:ring_buffer",
+ "//aos/containers:sized_array",
"//aos/network:message_bridge_server_fbs",
"//frc971/control_loops:profiled_subsystem_fbs",
"//frc971/control_loops/drivetrain:hybrid_ekf",
@@ -195,3 +205,19 @@
"@com_github_google_glog//:glog",
],
)
+
+ts_library(
+ name = "localizer_plotter",
+ srcs = ["localizer_plotter.ts"],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos:configuration_ts_fbs",
+ "//aos/network/www:aos_plotter",
+ "//aos/network/www:colors",
+ "//aos/network/www:plotter",
+ "//aos/network/www:proxy",
+ "//aos/network/www:reflection_ts",
+ "@com_github_google_flatbuffers//ts:flatbuffers_ts",
+ ],
+)
diff --git a/y2020/control_loops/drivetrain/drivetrain_replay.cc b/y2020/control_loops/drivetrain/drivetrain_replay.cc
index 65fc0c4..913fbda 100644
--- a/y2020/control_loops/drivetrain/drivetrain_replay.cc
+++ b/y2020/control_loops/drivetrain/drivetrain_replay.cc
@@ -75,6 +75,8 @@
"frc971.control_loops.drivetrain.Status");
reader.RemapLoggedChannel("/drivetrain",
"frc971.control_loops.drivetrain.Output");
+ reader.RemapLoggedChannel("/drivetrain",
+ "y2020.control_loops.drivetrain.LocalizerDebug");
reader.RemapLoggedChannel("/superstructure",
"y2020.control_loops.superstructure.Status");
reader.RemapLoggedChannel("/superstructure",
diff --git a/y2020/control_loops/drivetrain/localizer.cc b/y2020/control_loops/drivetrain/localizer.cc
index 906bb4b..9e0e83f 100644
--- a/y2020/control_loops/drivetrain/localizer.cc
+++ b/y2020/control_loops/drivetrain/localizer.cc
@@ -23,6 +23,10 @@
return result;
}
+// Offset to add to the pi's yaw in its extrinsics, to account for issues in the
+// calibrated extrinsics.
+constexpr double kTurretPiOffset = 0.0;
+
// Indices of the pis to use.
const std::array<std::string, 5> kPisToUse{"pi1", "pi2", "pi3", "pi4", "pi5"};
@@ -72,7 +76,11 @@
ekf_(dt_config),
clock_offset_fetcher_(
event_loop_->MakeFetcher<aos::message_bridge::ServerStatistics>(
- "/aos")) {
+ "/aos")),
+ debug_sender_(
+ event_loop_
+ ->MakeSender<y2020::control_loops::drivetrain::LocalizerDebug>(
+ "/drivetrain")) {
// TODO(james): The down estimator has trouble handling situations where the
// robot is constantly wiggling but not actually moving much, and can cause
// drift when using accelerometer readings.
@@ -145,17 +153,32 @@
double gyro_rate, const Eigen::Vector3d &accel) {
ekf_.UpdateEncodersAndGyro(left_encoder, right_encoder, gyro_rate,
U.cast<float>(), accel.cast<float>(), now);
+ auto builder = debug_sender_.MakeBuilder();
+ aos::SizedArray<flatbuffers::Offset<ImageMatchDebug>, 25> debug_offsets;
for (size_t ii = 0; ii < kPisToUse.size(); ++ii) {
auto &image_fetcher = image_fetchers_[ii];
while (image_fetcher.FetchNext()) {
- HandleImageMatch(kPisToUse[ii], *image_fetcher, now);
+ const auto offsets = HandleImageMatch(ii, kPisToUse[ii], *image_fetcher,
+ now, builder.fbb());
+ for (const auto offset : offsets) {
+ debug_offsets.push_back(offset);
+ }
}
}
+ const auto vector_offset =
+ builder.fbb()->CreateVector(debug_offsets.data(), debug_offsets.size());
+ LocalizerDebug::Builder debug_builder = builder.MakeBuilder<LocalizerDebug>();
+ debug_builder.add_matches(vector_offset);
+ CHECK(builder.Send(debug_builder.Finish()));
}
-void Localizer::HandleImageMatch(
- std::string_view pi, const frc971::vision::sift::ImageMatchResult &result,
- aos::monotonic_clock::time_point now) {
+aos::SizedArray<flatbuffers::Offset<ImageMatchDebug>, 5>
+Localizer::HandleImageMatch(
+ size_t camera_index, std::string_view pi,
+ const frc971::vision::sift::ImageMatchResult &result,
+ aos::monotonic_clock::time_point now, flatbuffers::FlatBufferBuilder *fbb) {
+ aos::SizedArray<flatbuffers::Offset<ImageMatchDebug>, 5> debug_offsets;
+
std::chrono::nanoseconds monotonic_offset(0);
clock_offset_fetcher_.Fetch();
if (clock_offset_fetcher_.get() != nullptr) {
@@ -177,11 +200,21 @@
<< capture_time;
if (capture_time > now) {
LOG(WARNING) << "Got camera frame from the future.";
- return;
+ ImageMatchDebug::Builder builder(*fbb);
+ builder.add_camera(camera_index);
+ builder.add_accepted(false);
+ builder.add_rejection_reason(RejectionReason::IMAGE_FROM_FUTURE);
+ debug_offsets.push_back(builder.Finish());
+ return debug_offsets;
}
if (!result.has_camera_calibration()) {
LOG(WARNING) << "Got camera frame without calibration data.";
- return;
+ ImageMatchDebug::Builder builder(*fbb);
+ builder.add_camera(camera_index);
+ builder.add_accepted(false);
+ builder.add_rejection_reason(RejectionReason::NO_CALIBRATION);
+ debug_offsets.push_back(builder.Finish());
+ return debug_offsets;
}
// Per the ImageMatchResult specification, we can actually determine whether
// the camera is the turret camera just from the presence of the
@@ -194,7 +227,12 @@
// seems reasonable, but may be unnecessarily low or high.
constexpr float kMaxTurretVelocity = 1.0;
if (is_turret && std::abs(turret_data.velocity) > kMaxTurretVelocity) {
- return;
+ ImageMatchDebug::Builder builder(*fbb);
+ builder.add_camera(camera_index);
+ builder.add_accepted(false);
+ builder.add_rejection_reason(RejectionReason::TURRET_TOO_FAST);
+ debug_offsets.push_back(builder.Finish());
+ return debug_offsets;
}
CHECK(result.camera_calibration()->has_fixed_extrinsics());
const Eigen::Matrix<float, 4, 4> fixed_extrinsics =
@@ -206,19 +244,38 @@
if (is_turret) {
H_robot_camera = H_robot_camera *
frc971::control_loops::TransformationMatrixForYaw<float>(
- turret_data.position) *
+ turret_data.position + kTurretPiOffset) *
FlatbufferToTransformationMatrix(
*result.camera_calibration()->turret_extrinsics());
}
if (!result.has_camera_poses()) {
- return;
+ ImageMatchDebug::Builder builder(*fbb);
+ builder.add_camera(camera_index);
+ builder.add_accepted(false);
+ builder.add_rejection_reason(RejectionReason::NO_RESULTS);
+ debug_offsets.push_back(builder.Finish());
+ return debug_offsets;
}
+ int index = -1;
for (const frc971::vision::sift::CameraPose *vision_result :
*result.camera_poses()) {
+ ++index;
+
+ ImageMatchDebug::Builder builder(*fbb);
+ builder.add_camera(camera_index);
+ builder.add_pose_index(index);
+ builder.add_local_image_capture_time_ns(result.image_monotonic_timestamp_ns());
+ builder.add_roborio_image_capture_time_ns(
+ capture_time.time_since_epoch().count());
+ builder.add_image_age_sec(aos::time::DurationInSeconds(now - capture_time));
+
if (!vision_result->has_camera_to_target() ||
!vision_result->has_field_to_target()) {
+ builder.add_accepted(false);
+ builder.add_rejection_reason(RejectionReason::NO_TRANSFORMS);
+ debug_offsets.push_back(builder.Finish());
continue;
}
const Eigen::Matrix<float, 4, 4> H_camera_target =
@@ -257,7 +314,7 @@
// populating some cross-correlation terms.
// Note that these are the noise standard deviations (they are squared below
// to get variances).
- Eigen::Matrix<float, 3, 1> noises(2.0, 2.0, 0.2);
+ Eigen::Matrix<float, 3, 1> noises(2.0, 2.0, 0.5);
// Augment the noise by the approximate rotational speed of the
// camera. This should help account for the fact that, while we are
// spinning, slight timing errors in the camera/turret data will tend to
@@ -271,9 +328,7 @@
H.setZero();
H(0, StateIdx::kX) = 1;
H(1, StateIdx::kY) = 1;
- // This is currently set to zero because we ignore the heading implied by
- // the camera.
- H(2, StateIdx::kTheta) = 0;
+ H(2, StateIdx::kTheta) = 1;
VLOG(1) << "Pose implied by target: " << Z.transpose()
<< " and current pose " << x() << ", " << y() << ", " << theta()
<< " Heading/dist/skew implied by target: "
@@ -282,6 +337,9 @@
// and don't use the correction.
if (std::abs(aos::math::DiffAngle<float>(theta(), Z(2))) > M_PI_2) {
AOS_LOG(WARNING, "Dropped image match due to heading mismatch.\n");
+ builder.add_accepted(false);
+ builder.add_rejection_reason(RejectionReason::HIGH_THETA_DIFFERENCE);
+ debug_offsets.push_back(builder.Finish());
continue;
}
// In order to do the EKF correction, we determine the expected state based
@@ -293,24 +351,42 @@
ekf_.LastStateBeforeTime(capture_time);
if (!state_at_capture.has_value()) {
AOS_LOG(WARNING, "Dropped image match due to age of image.\n");
+ builder.add_accepted(false);
+ builder.add_rejection_reason(RejectionReason::IMAGE_TOO_OLD);
+ debug_offsets.push_back(builder.Finish());
continue;
}
+
+ builder.add_implied_robot_x(Z(0));
+ builder.add_implied_robot_y(Z(1));
+ builder.add_implied_robot_theta(Z(2));
+
+ // Turret is zero when pointed backwards.
+ builder.add_implied_turret_goal(
+ aos::math::NormalizeAngle(M_PI + pose_robot_target.heading()));
+
+ std::optional<RejectionReason> correction_rejection;
const Input U = ekf_.MostRecentInput();
// For the correction step, instead of passing in the measurement directly,
// we pass in (0, 0, 0) as the measurement and then for the expected
// measurement (Zhat) we calculate the error between the implied and actual
// poses. This doesn't affect any of the math, it just makes the code a bit
// more convenient to write given the Correct() interface we already have.
+ // Note: If we start going back to doing back-in-time rewinds, then we can't
+ // get away with passing things by reference.
ekf_.Correct(
Eigen::Vector3f::Zero(), &U, {},
- [H, H_field_target, pose_robot_target, state_at_capture](
+ [H, H_field_target, pose_robot_target, state_at_capture, Z, &correction_rejection](
const State &, const Input &) -> Eigen::Vector3f {
- const Eigen::Vector3f Z =
+ // TODO(james): Figure out how to use the implied pose for...
+ const Eigen::Vector3f Z_implied =
CalculateImpliedPose(H_field_target, pose_robot_target);
+ (void)Z_implied;
// Just in case we ever do encounter any, drop measurements if they
// have non-finite numbers.
if (!Z.allFinite()) {
AOS_LOG(WARNING, "Got measurement with infinites or NaNs.\n");
+ correction_rejection = RejectionReason::NONFINITE_MEASUREMENT;
return Eigen::Vector3f::Zero();
}
Eigen::Vector3f Zhat = H * state_at_capture.value() - Z;
@@ -324,12 +400,21 @@
// because I primarily introduced it to make sure that any grossly
// invalid measurements get thrown out.
if (Zhat.squaredNorm() > std::pow(10.0, 2)) {
+ correction_rejection = RejectionReason::CORRECTION_TOO_LARGE;
return Eigen::Vector3f::Zero();
}
return Zhat;
},
[H](const State &) { return H; }, R, now);
+ if (correction_rejection) {
+ builder.add_accepted(false);
+ builder.add_rejection_reason(*correction_rejection);
+ } else {
+ builder.add_accepted(true);
+ }
+ debug_offsets.push_back(builder.Finish());
}
+ return debug_offsets;
}
} // namespace drivetrain
diff --git a/y2020/control_loops/drivetrain/localizer.h b/y2020/control_loops/drivetrain/localizer.h
index c3b6464..19ee4ad 100644
--- a/y2020/control_loops/drivetrain/localizer.h
+++ b/y2020/control_loops/drivetrain/localizer.h
@@ -4,11 +4,13 @@
#include <string_view>
#include "aos/containers/ring_buffer.h"
+#include "aos/containers/sized_array.h"
#include "aos/events/event_loop.h"
#include "aos/network/message_bridge_server_generated.h"
#include "frc971/control_loops/drivetrain/hybrid_ekf.h"
#include "frc971/control_loops/drivetrain/localizer.h"
#include "y2020/control_loops/superstructure/superstructure_status_generated.h"
+#include "y2020/control_loops/drivetrain/localizer_debug_generated.h"
#include "y2020/vision/sift/sift_generated.h"
namespace y2020 {
@@ -77,9 +79,11 @@
};
// Processes new image data from the given pi and updates the EKF.
- void HandleImageMatch(std::string_view pi,
- const frc971::vision::sift::ImageMatchResult &result,
- aos::monotonic_clock::time_point now);
+ aos::SizedArray<flatbuffers::Offset<ImageMatchDebug>, 5> HandleImageMatch(
+ size_t camera_index, std::string_view pi,
+ const frc971::vision::sift::ImageMatchResult &result,
+ aos::monotonic_clock::time_point now,
+ flatbuffers::FlatBufferBuilder *fbb);
// Processes the most recent turret position and stores it in the turret_data_
// buffer.
@@ -98,6 +102,8 @@
aos::Fetcher<aos::message_bridge::ServerStatistics> clock_offset_fetcher_;
+ aos::Sender<y2020::control_loops::drivetrain::LocalizerDebug> debug_sender_;
+
// Buffer of recent turret data--this is used so that when we receive a camera
// frame from the turret, we can back out what the turret angle was at that
// time.
diff --git a/y2020/control_loops/drivetrain/localizer_debug.fbs b/y2020/control_loops/drivetrain/localizer_debug.fbs
new file mode 100644
index 0000000..89aea68
--- /dev/null
+++ b/y2020/control_loops/drivetrain/localizer_debug.fbs
@@ -0,0 +1,34 @@
+namespace y2020.control_loops.drivetrain;
+
+enum RejectionReason : byte {
+ IMAGE_FROM_FUTURE = 0,
+ NO_CALIBRATION = 1,
+ TURRET_TOO_FAST = 2,
+ NO_RESULTS = 3,
+ NO_TRANSFORMS = 4,
+ HIGH_THETA_DIFFERENCE = 5,
+ IMAGE_TOO_OLD = 6,
+ NONFINITE_MEASUREMENT = 7,
+ CORRECTION_TOO_LARGE = 8,
+}
+
+table ImageMatchDebug {
+ camera:uint8 (id: 0);
+ pose_index: uint8 (id: 1);
+ local_image_capture_time_ns:long (id: 2);
+ roborio_image_capture_time_ns:long (id: 3);
+ implied_robot_x:float (id: 4);
+ implied_robot_y:float (id: 5);
+ implied_robot_theta:float (id: 6);
+ implied_turret_goal:float (id: 7);
+ accepted:bool (id: 8);
+ rejection_reason:RejectionReason (id: 9);
+ // Image age (more human-readable than trying to interpret roborio_image_capture_time_ns).
+ image_age_sec:float (id: 10);
+}
+
+table LocalizerDebug {
+ matches:[ImageMatchDebug] (id: 0);
+}
+
+root_type LocalizerDebug;
diff --git a/y2020/control_loops/drivetrain/localizer_plotter.ts b/y2020/control_loops/drivetrain/localizer_plotter.ts
new file mode 100644
index 0000000..4de2856
--- /dev/null
+++ b/y2020/control_loops/drivetrain/localizer_plotter.ts
@@ -0,0 +1,100 @@
+// Provides a plot for debugging robot state-related issues.
+import {AosPlotter} from 'org_frc971/aos/network/www/aos_plotter';
+import * as proxy from 'org_frc971/aos/network/www/proxy';
+import * as configuration from 'org_frc971/aos/configuration_generated';
+import {BLUE, BROWN, CYAN, GREEN, PINK, RED, WHITE} from 'org_frc971/aos/network/www/colors';
+import {MessageHandler, TimestampedMessage} from 'org_frc971/aos/network/www/aos_plotter';
+import {Point} from 'org_frc971/aos/network/www/plotter';
+import {Table} from 'org_frc971/aos/network/www/reflection';
+import {ByteBuffer} from 'org_frc971/external/com_github_google_flatbuffers/ts/byte-buffer';
+
+import Connection = proxy.Connection;
+import Schema = configuration.reflection.Schema;
+
+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 localizerDebug =
+ aosPlotter.addMessageSource('/drivetrain', 'y2020.control_loops.drivetrain.LocalizerDebug');
+ const imageMatch =
+ aosPlotter.addMessageSource('/pi1/camera', 'frc971.vision.sift.ImageMatchResult');
+
+ var currentTop = 0;
+
+ const imageAcceptedPlot = aosPlotter.addPlot(
+ element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ currentTop += DEFAULT_HEIGHT;
+ imageAcceptedPlot.plot.getAxisLabels().setTitle('Image Acceptance');
+ imageAcceptedPlot.plot.getAxisLabels().setXLabel(TIME);
+ imageAcceptedPlot.plot.getAxisLabels().setYLabel('[bool]');
+ imageAcceptedPlot.plot.setDefaultYRange([-0.05, 1.05]);
+
+ imageAcceptedPlot.addMessageLine(localizerDebug, ['matches[]', 'accepted'])
+ .setColor(RED)
+ .setDrawLine(false);
+
+ const impliedXPlot = aosPlotter.addPlot(
+ element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ currentTop += DEFAULT_HEIGHT;
+ impliedXPlot.plot.getAxisLabels().setTitle('Implied Robot X');
+ impliedXPlot.plot.getAxisLabels().setXLabel(TIME);
+ impliedXPlot.plot.getAxisLabels().setYLabel('[m]');
+
+ impliedXPlot.addMessageLine(localizerDebug, ['matches[]', 'implied_robot_x'])
+ .setColor(RED)
+ .setDrawLine(false);
+ impliedXPlot.addMessageLine(imageMatch, ['camera_poses[]', 'field_to_camera', 'data[3]'])
+ .setColor(BLUE)
+ .setDrawLine(false);
+
+ const impliedYPlot = aosPlotter.addPlot(
+ element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ currentTop += DEFAULT_HEIGHT;
+ impliedYPlot.plot.getAxisLabels().setTitle('Implied Robot Y');
+ impliedYPlot.plot.getAxisLabels().setXLabel(TIME);
+ impliedYPlot.plot.getAxisLabels().setYLabel('[m]');
+
+ impliedYPlot.addMessageLine(localizerDebug, ['matches[]', 'implied_robot_y'])
+ .setColor(RED)
+ .setDrawLine(false);
+ impliedYPlot.addMessageLine(imageMatch, ['camera_poses[]', 'field_to_camera', 'data[7]'])
+ .setColor(BLUE)
+ .setDrawLine(false);
+
+ const impliedHeadingPlot = aosPlotter.addPlot(
+ element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ currentTop += DEFAULT_HEIGHT;
+ impliedHeadingPlot.plot.getAxisLabels().setTitle('Implied Robot Theta');
+ impliedHeadingPlot.plot.getAxisLabels().setXLabel(TIME);
+ impliedHeadingPlot.plot.getAxisLabels().setYLabel('[rad]');
+
+ impliedHeadingPlot.addMessageLine(localizerDebug, ['matches[]', 'implied_robot_theta'])
+ .setColor(RED)
+ .setDrawLine(false);
+
+ const impliedTurretGoalPlot = aosPlotter.addPlot(
+ element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ currentTop += DEFAULT_HEIGHT;
+ impliedTurretGoalPlot.plot.getAxisLabels().setTitle('Implied Turret Goal');
+ impliedTurretGoalPlot.plot.getAxisLabels().setXLabel(TIME);
+ impliedTurretGoalPlot.plot.getAxisLabels().setYLabel('[rad]');
+
+ impliedTurretGoalPlot.addMessageLine(localizerDebug, ['matches[]', 'implied_turret_goal'])
+ .setColor(RED)
+ .setDrawLine(false);
+
+ const imageTimingPlot = aosPlotter.addPlot(
+ element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ currentTop += DEFAULT_HEIGHT;
+ imageTimingPlot.plot.getAxisLabels().setTitle('Timing Plot');
+ imageTimingPlot.plot.getAxisLabels().setXLabel(TIME);
+ imageTimingPlot.plot.getAxisLabels().setYLabel('[ns]');
+
+ imageTimingPlot.addMessageLine(localizerDebug, ['matches[]', 'image_age_sec'])
+ .setColor(RED)
+ .setDrawLine(false);
+}
diff --git a/y2020/control_loops/superstructure/BUILD b/y2020/control_loops/superstructure/BUILD
index 2d47cf4..db8a060 100644
--- a/y2020/control_loops/superstructure/BUILD
+++ b/y2020/control_loops/superstructure/BUILD
@@ -154,9 +154,13 @@
srcs = ["turret_plotter.ts"],
target_compatible_with = ["@platforms//os:linux"],
deps = [
+ "//aos:configuration_ts_fbs",
"//aos/network/www:aos_plotter",
"//aos/network/www:colors",
+ "//aos/network/www:plotter",
"//aos/network/www:proxy",
+ "//aos/network/www:reflection_ts",
+ "@com_github_google_flatbuffers//ts:flatbuffers_ts",
],
)
diff --git a/y2020/control_loops/superstructure/superstructure.cc b/y2020/control_loops/superstructure/superstructure.cc
index ce816a0..98de31d 100644
--- a/y2020/control_loops/superstructure/superstructure.cc
+++ b/y2020/control_loops/superstructure/superstructure.cc
@@ -164,11 +164,16 @@
if (output != nullptr) {
// Friction is a pain and putting a really high burden on the integrator.
+ // TODO(james): I'm not sure how helpful this gain is.
const double turret_velocity_sign =
turret_status->velocity() * kTurretFrictionGain;
output_struct.turret_voltage +=
std::clamp(turret_velocity_sign, -kTurretFrictionVoltageLimit,
kTurretFrictionVoltageLimit);
+ const double time_sec =
+ aos::time::DurationInSeconds(position_timestamp.time_since_epoch());
+ output_struct.turret_voltage +=
+ kTurretDitherGain * std::sin(2.0 * M_PI * time_sec * 30.0);
output_struct.turret_voltage =
std::clamp(output_struct.turret_voltage, -turret_.operating_voltage(),
turret_.operating_voltage());
diff --git a/y2020/control_loops/superstructure/superstructure.h b/y2020/control_loops/superstructure/superstructure.h
index c03f7a7..e4a9cb6 100644
--- a/y2020/control_loops/superstructure/superstructure.h
+++ b/y2020/control_loops/superstructure/superstructure.h
@@ -28,6 +28,7 @@
// voltage cap.
static constexpr double kTurretFrictionGain = 10.0;
static constexpr double kTurretFrictionVoltageLimit = 1.5;
+ static constexpr double kTurretDitherGain = 0.4;
using PotAndAbsoluteEncoderSubsystem =
::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystem<
diff --git a/y2020/control_loops/superstructure/superstructure_lib_test.cc b/y2020/control_loops/superstructure/superstructure_lib_test.cc
index 22dd40e..112780a 100644
--- a/y2020/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2020/control_loops/superstructure/superstructure_lib_test.cc
@@ -742,7 +742,7 @@
superstructure_plant_.set_peak_intake_acceleration(0.2);
superstructure_plant_.set_peak_turret_velocity(23.0);
- superstructure_plant_.set_peak_turret_acceleration(0.2);
+ superstructure_plant_.set_peak_turret_acceleration(6.0);
// Intake needs over 9 seconds to reach the goal
RunFor(chrono::seconds(10));
@@ -971,7 +971,7 @@
superstructure_status_fetcher_.Fetch();
EXPECT_NEAR(kShotAngle, superstructure_status_fetcher_->turret()->position(),
- 1e-4);
+ 5e-4);
EXPECT_FLOAT_EQ(kShotAngle,
superstructure_status_fetcher_->aimer()->turret_position());
EXPECT_FLOAT_EQ(0,
diff --git a/y2020/control_loops/superstructure/turret_plotter.ts b/y2020/control_loops/superstructure/turret_plotter.ts
index 3d975d8..948ca97 100644
--- a/y2020/control_loops/superstructure/turret_plotter.ts
+++ b/y2020/control_loops/superstructure/turret_plotter.ts
@@ -1,29 +1,82 @@
// Provides a plot for debugging robot state-related issues.
import {AosPlotter} from 'org_frc971/aos/network/www/aos_plotter';
import * as proxy from 'org_frc971/aos/network/www/proxy';
+import * as configuration from 'org_frc971/aos/configuration_generated';
import {BLUE, BROWN, CYAN, GREEN, PINK, RED, WHITE} from 'org_frc971/aos/network/www/colors';
+import {MessageHandler, TimestampedMessage} from 'org_frc971/aos/network/www/aos_plotter';
+import {Point} from 'org_frc971/aos/network/www/plotter';
+import {Table} from 'org_frc971/aos/network/www/reflection';
+import {ByteBuffer} from 'org_frc971/external/com_github_google_flatbuffers/ts/byte-buffer';
import Connection = proxy.Connection;
+import Schema = configuration.reflection.Schema;
const TIME = AosPlotter.TIME;
const DEFAULT_WIDTH = AosPlotter.DEFAULT_WIDTH;
const DEFAULT_HEIGHT = AosPlotter.DEFAULT_HEIGHT;
+class DerivativeMessageHandler extends MessageHandler {
+ // Calculated magnitude of the measured acceleration from the IMU.
+ private acceleration_magnitudes: Point[] = [];
+ constructor(private readonly schema: Schema) {
+ super(schema);
+ }
+ private readScalar(table: Table, fieldName: string): number {
+ return this.parser.readScalar(table, fieldName);
+ }
+
+ // Computes a numerical derivative for a given input.
+ private derivative(input: Point[]): Point[] {
+ const num_measurements = input.length;
+ const results = [];
+ for (let ii = 0; ii < num_measurements - 1; ++ii) {
+ const x0 = input[ii].x;
+ const x1 = input[ii + 1].x;
+ const y0 = input[ii].y;
+ const y1 = input[ii + 1].y;
+ results.push(new Point((x0 + x1) / 2.0, (y1 - y0) / (x1 - x0)));
+ }
+ return results;
+ }
+
+ getField(field: string[]): Point[] {
+ // Any requested input that ends with "_derivative" will get a derivative
+ // calculated for the provided field.
+ const derivative_suffix = "_derivative";
+ const num_fields = field.length;
+ const end_field = field[num_fields - 1];
+ if (end_field.endsWith(derivative_suffix)) {
+ const field_copy = [];
+ for (let ii = 0; ii < num_fields - 1; ++ii) {
+ field_copy.push(field[ii]);
+ }
+ field_copy.push(end_field.slice(0, end_field.length - derivative_suffix.length));
+ return this.derivative(this.getField(field_copy));
+ } else {
+ return super.getField(field);
+ }
+ }
+}
+
export function plotTurret(conn: Connection, element: Element) : void {
const aosPlotter = new AosPlotter(conn);
const goal = aosPlotter.addMessageSource('/superstructure', 'y2020.control_loops.superstructure.Goal');
const output = aosPlotter.addMessageSource('/superstructure', 'y2020.control_loops.superstructure.Output');
- const status = aosPlotter.addMessageSource(
- '/superstructure', 'y2020.control_loops.superstructure.Status');
+ const status = aosPlotter.addRawMessageSource(
+ '/superstructure', 'y2020.control_loops.superstructure.Status',
+ new DerivativeMessageHandler(conn.getSchema('y2020.control_loops.superstructure.Status'))
+ );
const pdpValues =
aosPlotter.addMessageSource('/roborio/aos', 'frc971.PDPValues');
+ const localizerDebug =
+ aosPlotter.addMessageSource('/drivetrain', 'y2020.control_loops.drivetrain.LocalizerDebug');
var currentTop = 0;
const turretPosPlot = aosPlotter.addPlot(
element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
currentTop += DEFAULT_HEIGHT;
- turretPosPlot.plot.getAxisLabels().setTitle('Turret Goal Position');
+ turretPosPlot.plot.getAxisLabels().setTitle('Turret Position');
turretPosPlot.plot.getAxisLabels().setXLabel(TIME);
turretPosPlot.plot.getAxisLabels().setYLabel('rad');
@@ -33,8 +86,39 @@
turretPosPlot.addMessageLine(status, ['turret', 'position'])
.setColor(GREEN)
.setPointSize(0.0);
+ turretPosPlot.addMessageLine(localizerDebug, ['matches[]', 'implied_turret_goal'])
+ .setColor(GREEN)
+ .setDrawLine(false);
turretPosPlot.addMessageLine(status, ['turret', 'unprofiled_goal_position'])
.setColor(BLUE)
+ .setDrawLine(false);
+
+ const turretVelPlot = aosPlotter.addPlot(
+ element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ currentTop += DEFAULT_HEIGHT;
+ turretVelPlot.plot.getAxisLabels().setTitle('Turret Velocity');
+ turretVelPlot.plot.getAxisLabels().setXLabel(TIME);
+ turretVelPlot.plot.getAxisLabels().setYLabel('rad / sec');
+
+ turretVelPlot.addMessageLine(status, ['aimer', 'turret_velocity'])
+ .setColor(RED)
+ .setPointSize(0.0);
+ turretVelPlot.addMessageLine(status, ['turret', 'velocity'])
+ .setColor(GREEN)
+ .setPointSize(0.0);
+ turretVelPlot.addMessageLine(status, ['turret', 'unprofiled_goal_velocity'])
+ .setColor(BLUE)
+ .setDrawLine(false);
+
+ const turretAccelPlot = aosPlotter.addPlot(
+ element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ currentTop += DEFAULT_HEIGHT;
+ turretAccelPlot.plot.getAxisLabels().setTitle('Turret Acceleration');
+ turretAccelPlot.plot.getAxisLabels().setXLabel(TIME);
+ turretAccelPlot.plot.getAxisLabels().setYLabel('rad / sec / sec');
+
+ turretAccelPlot.addMessageLine(status, ['aimer', 'turret_velocity_derivative'])
+ .setColor(RED)
.setPointSize(0.0);
const turretVoltagePlot = aosPlotter.addPlot(
@@ -44,6 +128,15 @@
turretVoltagePlot.plot.getAxisLabels().setXLabel(TIME);
turretVoltagePlot.plot.getAxisLabels().setYLabel('V');
+ turretVoltagePlot.addMessageLine(status, ['turret', 'voltage_error'])
+ .setColor(GREEN)
+ .setPointSize(0.0);
+ turretVoltagePlot.addMessageLine(status, ['turret', 'position_power'])
+ .setColor(BLUE)
+ .setPointSize(0.0);
+ turretVoltagePlot.addMessageLine(status, ['turret', 'velocity_power'])
+ .setColor(CYAN)
+ .setPointSize(0.0);
turretVoltagePlot.addMessageLine(output, ['turret_voltage'])
.setColor(RED)
.setPointSize(0.0);
@@ -54,7 +147,7 @@
currentPlot.plot.getAxisLabels().setTitle('Current');
currentPlot.plot.getAxisLabels().setXLabel(TIME);
currentPlot.plot.getAxisLabels().setYLabel('Amps');
- currentPlot.plot.setDefaultYRange([0.0, 80.0]);
+ currentPlot.plot.setDefaultYRange([0.0, 40.0]);
currentPlot.addMessageLine(pdpValues, ['currents[6]'])
.setColor(GREEN)
@@ -83,4 +176,16 @@
targetChoicePlot.addMessageLine(status, ['aimer', 'aiming_for_inner_port'])
.setColor(RED)
.setPointSize(0.0);
+
+ const imageAcceptedPlot = aosPlotter.addPlot(
+ element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ currentTop += DEFAULT_HEIGHT;
+ imageAcceptedPlot.plot.getAxisLabels().setTitle('Image Acceptance');
+ imageAcceptedPlot.plot.getAxisLabels().setXLabel(TIME);
+ imageAcceptedPlot.plot.getAxisLabels().setYLabel('[bool]');
+ imageAcceptedPlot.plot.setDefaultYRange([-0.05, 1.05]);
+
+ imageAcceptedPlot.addMessageLine(localizerDebug, ['matches[]', 'accepted'])
+ .setColor(RED)
+ .setDrawLine(false);
}
diff --git a/y2020/joystick_reader.cc b/y2020/joystick_reader.cc
index 44d8a42..bb57997 100644
--- a/y2020/joystick_reader.cc
+++ b/y2020/joystick_reader.cc
@@ -101,6 +101,7 @@
double hood_pos = constants::Values::kHoodRange().middle();
double intake_pos = -0.89;
+ double turret_pos = 0.0;
float roller_speed = 0.0f;
float roller_speed_compensation = 0.0f;
double accelerator_speed = 0.0;
@@ -119,6 +120,12 @@
}
}
+ if (setpoint_fetcher_.get()) {
+ turret_pos = setpoint_fetcher_->turret();
+ } else {
+ turret_pos = 0.0;
+ }
+
if (data.IsPressed(kShootFast)) {
if (setpoint_fetcher_.get()) {
accelerator_speed = setpoint_fetcher_->accelerator();
@@ -175,7 +182,7 @@
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
- *builder.fbb(), 0.0,
+ *builder.fbb(), turret_pos,
CreateProfileParameters(*builder.fbb(), 6.0, 20.0));
flatbuffers::Offset<superstructure::ShooterGoal> shooter_offset =
diff --git a/y2020/setpoint.fbs b/y2020/setpoint.fbs
index 74164e5..9a9a0be 100644
--- a/y2020/setpoint.fbs
+++ b/y2020/setpoint.fbs
@@ -6,6 +6,8 @@
finisher:double (id: 1);
hood:double (id: 2);
+
+ turret:double (id: 3);
}
root_type Setpoint;
diff --git a/y2020/setpoint_setter.cc b/y2020/setpoint_setter.cc
index 3f493c9..1e1a7b8 100644
--- a/y2020/setpoint_setter.cc
+++ b/y2020/setpoint_setter.cc
@@ -6,6 +6,7 @@
DEFINE_double(accelerator, 250.0, "Accelerator speed");
DEFINE_double(finisher, 500.0, "Finsher speed");
DEFINE_double(hood, 0.45, "Hood setpoint");
+DEFINE_double(turret, 0.0, "Turret setpoint");
int main(int argc, char **argv) {
aos::InitGoogle(&argc, &argv);
@@ -27,6 +28,7 @@
setpoint_builder.add_accelerator(FLAGS_accelerator);
setpoint_builder.add_finisher(FLAGS_finisher);
setpoint_builder.add_hood(FLAGS_hood);
+ setpoint_builder.add_turret(FLAGS_turret);
builder.Send(setpoint_builder.Finish());
return 0;
diff --git a/y2020/vision/tools/python_code/calib_files/calibration_pi-971-1_2021-09-11_17-49-00.000000000.json b/y2020/vision/tools/python_code/calib_files/calibration_pi-971-1_2021-09-11_17-49-00.000000000.json
index a144853..39c7911 100644
--- a/y2020/vision/tools/python_code/calib_files/calibration_pi-971-1_2021-09-11_17-49-00.000000000.json
+++ b/y2020/vision/tools/python_code/calib_files/calibration_pi-971-1_2021-09-11_17-49-00.000000000.json
@@ -1,6 +1,6 @@
{
"node_name": "pi1",
- "team_number": 7971,
+ "team_number": 971,
"intrinsics": [
392.276093,
0.0,
diff --git a/y2020/vision/tools/python_code/calib_files/reference_cal_pi_cam.json b/y2020/vision/tools/python_code/calib_files/reference_cal_pi_cam.json
new file mode 100644
index 0000000..8c9472e
--- /dev/null
+++ b/y2020/vision/tools/python_code/calib_files/reference_cal_pi_cam.json
@@ -0,0 +1,23 @@
+{
+ "node_name": "reference",
+ "team_number": 1971,
+ "intrinsics": [
+ 388.0,
+ 0.0,
+ 320.0,
+ 0.0,
+ 388.0,
+ 240.0,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "dist_coeffs": [
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0,
+ 0.0
+ ],
+ "calibration_timestamp": 1597994992500905688
+}
diff --git a/y2020/vision/tools/python_code/camera_definition.py b/y2020/vision/tools/python_code/camera_definition.py
index 33eb1a9..6172ae7 100644
--- a/y2020/vision/tools/python_code/camera_definition.py
+++ b/y2020/vision/tools/python_code/camera_definition.py
@@ -7,7 +7,7 @@
import define_training_data as dtd
-glog.setLevel("WARN")
+glog.setLevel("INFO")
class CameraIntrinsics:
@@ -36,24 +36,14 @@
def load_camera_definitions():
### CAMERA DEFINITIONS
+ # We only load in cameras that have a calibration file
+ # These are stored in y2020/vision/tools/python_code/calib_files
+ # See reference_calibration_pi_cam.json for an example default file
+ #
+ # Or better yet, use //y2020/vision:calibration to calibrate the camera
+ # using a Charuco target board
- # Robot camera has:
- # FOV_H = 93.*math.pi()/180.
- # FOV_V = 70.*math.pi()/180.
-
- # Create fake camera (based on USB webcam params)
- fx = 810.
- fy = 810.
- cx = 320.
- cy = 240.
-
- # Define a web_cam
- web_cam_int = CameraIntrinsics()
- web_cam_int.camera_matrix = np.asarray([[fx, 0, cx], [0, fy, cy],
- [0, 0, 1]])
- web_cam_int.dist_coeffs = np.zeros((5, 1))
-
- web_cam_ext = CameraExtrinsics()
+ # Extrinsic definition
# Camera rotation from robot x,y,z to opencv (z, -x, -y)
# This is extrinsics for the turret camera
# camera pose relative to center, base of the turret
@@ -63,37 +53,30 @@
[[np.cos(camera_pitch), 0.0, -np.sin(camera_pitch)], [0.0, 1.0, 0.0],
[np.sin(camera_pitch), 0.0,
np.cos(camera_pitch)]])
- web_cam_ext.R = np.array(
+ turret_cam_ext = CameraExtrinsics()
+ turret_cam_ext.R = np.array(
camera_pitch_matrix *
np.matrix([[0., 0., 1.], [-1, 0, 0], [0, -1., 0]]))
- #web_cam_ext.T = np.array([0., 0., 0.])
- web_cam_ext.T = np.array([2.0 * 0.0254, -6.0 * 0.0254, 41.0 * 0.0254])
- fixed_ext = CameraExtrinsics()
- fixed_ext.R = np.array([[-1.0, 0.0, 0.0], [0.0, -1.0, 0.0],
- [0.0, 0.0, 1.0]])
- fixed_ext.T = np.array([0.0, 0.0, 0.0])
+ turret_cam_ext.T = np.array([15.0 * 0.0254, 15.0 * 0.0254, 41.0 * 0.0254])
+ default_cam_ext = CameraExtrinsics()
+ default_cam_ext.R = np.array([[-1.0, 0.0, 0.0], [0.0, -1.0, 0.0],
+ [0.0, 0.0, 1.0]])
+ default_cam_ext.T = np.array([0.0, 0.0, 0.0])
- web_cam_params = CameraParameters()
- web_cam_params.camera_int = web_cam_int
- # Fixed extrinsics are for the turret, which is centered on the robot and
- # pointed straight backwards.
- web_cam_params.camera_ext = fixed_ext
- web_cam_params.turret_ext = web_cam_ext
+ default_cam_params = CameraParameters()
+ # Currently, all cameras have this same set of extrinsics
+ default_cam_params.camera_ext = default_cam_ext
+ default_cam_params.turret_ext = turret_cam_ext
camera_list = []
- # TODO<Jim>: Should probably make this a dict to make replacing easier
- for team_number in (971, 7971, 8971, 9971):
- for node_name in ("pi0", "pi1", "pi2", "pi3", "pi4", "pi5"):
- camera_base = copy.deepcopy(web_cam_params)
- camera_base.node_name = node_name
- camera_base.team_number = team_number
- camera_list.append(camera_base)
-
dir_name = dtd.bazel_name_fix('calib_files')
+ glog.debug("Searching for calibration files in " + dir_name)
for filename in os.listdir(dir_name):
glog.debug("Inspecting %s", filename)
- if ("cam-calib-int" in filename or 'calibration' in filename) and filename.endswith(".json"):
+ if ("cam-calib-int" in filename
+ or 'calibration' in filename) and filename.endswith(".json"):
+
# Extract intrinsics from file
calib_file = open(dir_name + "/" + filename, 'r')
calib_dict = json.loads(calib_file.read())
@@ -104,6 +87,8 @@
# JSON.
# See which one we have and parse accordingly.
if 'hostname' in calib_dict:
+ glog.warn("WARNING: Using older calibration file.")
+ glog.warn("Preferred usage is y2020/vision:calibration")
hostname_split = calib_dict["hostname"].split("-")
team_number = int(hostname_split[1])
node_name = hostname_split[0] + hostname_split[2]
@@ -117,14 +102,13 @@
dist_coeffs = np.asarray(calib_dict["dist_coeffs"]).reshape(
(1, 5))
- # Look for match, and replace camera_intrinsics
- for camera_calib in camera_list:
- if camera_calib.node_name == node_name and camera_calib.team_number == team_number:
- glog.info("Found calib for %s, team #%d" % (node_name,
- team_number))
- camera_calib.camera_int.camera_matrix = copy.copy(
- camera_matrix)
- camera_calib.camera_int.dist_coeffs = copy.copy(
- dist_coeffs)
+ glog.info("Found calib for " + node_name + ", team #" +
+ str(team_number))
+ camera_base = copy.deepcopy(default_cam_params)
+ camera_base.node_name = node_name
+ camera_base.team_number = team_number
+ camera_base.camera_int.camera_matrix = copy.copy(camera_matrix)
+ camera_base.camera_int.dist_coeffs = copy.copy(dist_coeffs)
+ camera_list.append(camera_base)
return camera_list
diff --git a/y2020/www/field_handler.ts b/y2020/www/field_handler.ts
index 4aaa05f..1a0b56a 100644
--- a/y2020/www/field_handler.ts
+++ b/y2020/www/field_handler.ts
@@ -191,6 +191,8 @@
ctx.beginPath();
ctx.moveTo(0.5, 0.5);
ctx.lineTo(0, 0);
+ ctx.lineTo(100.0, 0);
+ ctx.lineTo(0, 0);
ctx.lineTo(0.5, -0.5);
ctx.stroke();
ctx.beginPath();
@@ -218,13 +220,13 @@
// Draw line in circle to show forwards.
ctx.beginPath();
ctx.moveTo(0, 0);
- ctx.lineTo(turretRadius, 0);
+ ctx.lineTo(1000.0 * turretRadius, 0);
ctx.stroke();
ctx.restore();
}
ctx.beginPath();
ctx.moveTo(0, 0);
- ctx.lineTo(ROBOT_LENGTH / 2, 0);
+ ctx.lineTo(100.0 * ROBOT_LENGTH / 2, 0);
ctx.stroke();
ctx.restore();
}
diff --git a/y2020/y2020_roborio.json b/y2020/y2020_roborio.json
index 5243d5a..db5da14 100644
--- a/y2020/y2020_roborio.json
+++ b/y2020/y2020_roborio.json
@@ -196,6 +196,14 @@
},
{
"name": "/drivetrain",
+ "type": "y2020.control_loops.drivetrain.LocalizerDebug",
+ "source_node": "roborio",
+ "frequency": 250,
+ "max_size": 512,
+ "num_senders": 2
+ },
+ {
+ "name": "/drivetrain",
"type": "frc971.IMUValuesBatch",
"source_node": "roborio",
"frequency": 250,
diff --git a/y2021_bot3/control_loops/python/BUILD b/y2021_bot3/control_loops/python/BUILD
index 7ade878..15dacaa 100644
--- a/y2021_bot3/control_loops/python/BUILD
+++ b/y2021_bot3/control_loops/python/BUILD
@@ -5,8 +5,10 @@
srcs = [
"drivetrain.py",
],
+ legacy_create_init = False,
target_compatible_with = ["@platforms//cpu:x86_64"],
deps = [
+ ":python_init",
"//external:python-gflags",
"//external:python-glog",
"//frc971/control_loops/python:drivetrain",
@@ -19,8 +21,10 @@
"drivetrain.py",
"polydrivetrain.py",
],
+ legacy_create_init = False,
target_compatible_with = ["@platforms//cpu:x86_64"],
deps = [
+ ":python_init",
"//external:python-gflags",
"//external:python-glog",
"//frc971/control_loops/python:polydrivetrain",
@@ -43,3 +47,11 @@
"//frc971/control_loops/python:polydrivetrain",
],
)
+
+py_library(
+ name = "python_init",
+ srcs = ["__init__.py"],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = ["//y2020/control_loops:python_init"],
+)
diff --git a/y2021_bot3/control_loops/python/__init__.py b/y2021_bot3/control_loops/python/__init__.py
new file mode 100644
index 0000000..e69de29
--- /dev/null
+++ b/y2021_bot3/control_loops/python/__init__.py
diff --git a/y2021_bot3/control_loops/superstructure/BUILD b/y2021_bot3/control_loops/superstructure/BUILD
index 404555f..77cdf45 100644
--- a/y2021_bot3/control_loops/superstructure/BUILD
+++ b/y2021_bot3/control_loops/superstructure/BUILD
@@ -76,3 +76,29 @@
"//aos/events:shm_event_loop",
],
)
+
+cc_test(
+ name = "superstructure_lib_test",
+ srcs = [
+ "superstructure_lib_test.cc",
+ ],
+ data = [
+ "//y2021_bot3:config",
+ ],
+ deps = [
+ ":superstructure_goal_fbs",
+ ":superstructure_lib",
+ ":superstructure_output_fbs",
+ ":superstructure_position_fbs",
+ ":superstructure_status_fbs",
+ "//aos:math",
+ "//aos/events/logging:log_writer",
+ "//aos/testing:googletest",
+ "//aos/time",
+ "//frc971/control_loops:capped_test_plant",
+ "//frc971/control_loops:control_loop_test",
+ "//frc971/control_loops:position_sensor_sim",
+ "//frc971/control_loops:team_number_test_environment",
+ "//frc971/control_loops/drivetrain:drivetrain_status_fbs",
+ ],
+)
\ No newline at end of file
diff --git a/y2021_bot3/control_loops/superstructure/superstructure.cc b/y2021_bot3/control_loops/superstructure/superstructure.cc
index aaf3def..22c7c44 100644
--- a/y2021_bot3/control_loops/superstructure/superstructure.cc
+++ b/y2021_bot3/control_loops/superstructure/superstructure.cc
@@ -16,7 +16,7 @@
event_loop->SetRuntimeRealtimePriority(30);
}
-void Superstructure::RunIteration(const Goal * /*unsafe_goal*/,
+void Superstructure::RunIteration(const Goal *unsafe_goal,
const Position * /*position*/,
aos::Sender<Output>::Builder *output,
aos::Sender<Status>::Builder *status) {
@@ -24,8 +24,13 @@
AOS_LOG(ERROR, "WPILib reset, restarting\n");
}
- if (output != nullptr) {
+ if (output != nullptr && unsafe_goal != nullptr) {
OutputT output_struct;
+
+ output_struct.intake_volts =
+ std::clamp(unsafe_goal->intake_speed(), -12.0, 12.0);
+ output_struct.outtake_volts =
+ std::clamp(unsafe_goal->outtake_speed(), -12.0, 12.0);
output->Send(Output::Pack(*output->fbb(), &output_struct));
}
@@ -34,6 +39,11 @@
status_builder.add_zeroed(true);
status_builder.add_estopped(false);
+ if (unsafe_goal != nullptr) {
+ status_builder.add_intake_speed(unsafe_goal->intake_speed());
+ status_builder.add_outtake_speed(unsafe_goal->outtake_speed());
+ }
+
status->Send(status_builder.Finish());
}
diff --git a/y2021_bot3/control_loops/superstructure/superstructure_lib_test.cc b/y2021_bot3/control_loops/superstructure/superstructure_lib_test.cc
new file mode 100644
index 0000000..cd031c3
--- /dev/null
+++ b/y2021_bot3/control_loops/superstructure/superstructure_lib_test.cc
@@ -0,0 +1,158 @@
+#include <chrono>
+#include <memory>
+
+#include "aos/events/logging/log_writer.h"
+#include "frc971/control_loops/capped_test_plant.h"
+#include "frc971/control_loops/control_loop_test.h"
+#include "frc971/control_loops/position_sensor_sim.h"
+#include "frc971/control_loops/team_number_test_environment.h"
+#include "gtest/gtest.h"
+#include "y2021_bot3/control_loops/superstructure/superstructure.h"
+
+namespace y2021_bot3 {
+namespace control_loops {
+namespace superstructure {
+namespace testing {
+
+class SuperstructureTest : public ::frc971::testing::ControlLoopTest {
+ public:
+ SuperstructureTest()
+ : ::frc971::testing::ControlLoopTest(
+ aos::configuration::ReadConfig("y2021_bot3/config.json"),
+ std::chrono::microseconds(5050)),
+ superstructure_event_loop(MakeEventLoop("Superstructure")),
+ superstructure_(superstructure_event_loop.get()),
+ test_event_loop_(MakeEventLoop("test")),
+ superstructure_goal_fetcher_(
+ test_event_loop_->MakeFetcher<Goal>("/superstructure")),
+ superstructure_goal_sender_(
+ test_event_loop_->MakeSender<Goal>("/superstructure")),
+ superstructure_status_fetcher_(
+ test_event_loop_->MakeFetcher<Status>("/superstructure")),
+ superstructure_output_fetcher_(
+ test_event_loop_->MakeFetcher<Output>("/superstructure")),
+ superstructure_position_fetcher_(
+ test_event_loop_->MakeFetcher<Position>("/superstructure")),
+ superstructure_position_sender_(
+ test_event_loop_->MakeSender<Position>("/superstructure")) {
+ set_team_id(frc971::control_loops::testing::kTeamNumber);
+ SetEnabled(true);
+
+ phased_loop_handle_ = test_event_loop_->AddPhasedLoop(
+ [this](int) { SendPositionMessage(); }, dt());
+ }
+
+ void VerifyResults(double intake_voltage, double outtake_voltage,
+ double intake_speed, double outtake_speed) {
+ superstructure_output_fetcher_.Fetch();
+ superstructure_status_fetcher_.Fetch();
+ ASSERT_TRUE(superstructure_output_fetcher_.get() != nullptr);
+ ASSERT_TRUE(superstructure_status_fetcher_.get() != nullptr);
+ EXPECT_EQ(superstructure_output_fetcher_->intake_volts(), intake_voltage);
+ EXPECT_EQ(superstructure_output_fetcher_->outtake_volts(), outtake_voltage);
+ EXPECT_EQ(superstructure_status_fetcher_->intake_speed(), intake_speed);
+ EXPECT_EQ(superstructure_status_fetcher_->outtake_speed(), outtake_speed);
+ EXPECT_EQ(superstructure_status_fetcher_->estopped(), false);
+ EXPECT_EQ(superstructure_status_fetcher_->zeroed(), true);
+ }
+
+ void SendPositionMessage() {
+ auto builder = superstructure_position_sender_.MakeBuilder();
+ Position::Builder position_builder = builder.MakeBuilder<Position>();
+ builder.Send(position_builder.Finish());
+ }
+
+ ::std::unique_ptr<::aos::EventLoop> superstructure_event_loop;
+ ::y2021_bot3::control_loops::superstructure::Superstructure superstructure_;
+ ::std::unique_ptr<::aos::EventLoop> test_event_loop_;
+ ::aos::PhasedLoopHandler *phased_loop_handle_ = nullptr;
+ ::aos::Fetcher<Goal> superstructure_goal_fetcher_;
+ ::aos::Sender<Goal> superstructure_goal_sender_;
+ ::aos::Fetcher<Status> superstructure_status_fetcher_;
+ ::aos::Fetcher<Output> superstructure_output_fetcher_;
+ ::aos::Fetcher<Position> superstructure_position_fetcher_;
+ ::aos::Sender<Position> superstructure_position_sender_;
+};
+
+// Tests running the intake and outtake separately
+TEST_F(SuperstructureTest, RunIntakeOrOuttake) {
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake_speed(10.0);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ SendPositionMessage();
+ RunFor(dt() * 2);
+ VerifyResults(10.0, 0.0, 10.0, 0.0);
+ }
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_outtake_speed(10.0);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ RunFor(dt() * 2);
+ VerifyResults(0.0, 10.0, 0.0, 10.0);
+ }
+}
+
+// Tests running both the intake and the outtake simultaneously
+TEST_F(SuperstructureTest, RunIntakeAndOuttake) {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake_speed(10.0);
+ goal_builder.add_outtake_speed(5.0);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ RunFor(dt() * 2);
+ VerifyResults(10.0, 5.0, 10.0, 5.0);
+}
+
+// Tests for an invalid voltage (over 12 or under -12) to check that it defaults
+// to 12 or -12
+TEST_F(SuperstructureTest, InvalidVoltage) {
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake_speed(20.0);
+ goal_builder.add_outtake_speed(15.0);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ RunFor(dt() * 2);
+ VerifyResults(12.0, 12.0, 20.0, 15.0);
+ }
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake_speed(-20.0);
+ goal_builder.add_outtake_speed(-15.0);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ RunFor(dt() * 2);
+ VerifyResults(-12.0, -12.0, -20.0, -15.0);
+ }
+}
+
+// Tests that there is no output when the goal is null
+TEST_F(SuperstructureTest, GoalIsNull) {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ RunFor(dt() * 2);
+ VerifyResults(0.0, 0.0, 0.0, 0.0);
+}
+
+// Tests that the robot behaves properly when disabled
+TEST_F(SuperstructureTest, Disabled) {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake_speed(6.0);
+ goal_builder.add_outtake_speed(5.0);
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
+ SetEnabled(false);
+ RunFor(dt() * 2);
+ VerifyResults(0.0, 0.0, 6.0, 5.0);
+}
+
+} // namespace testing
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2021_bot3
\ No newline at end of file
