Merge "Provide better error message for missing timestamp channel in message bridge"
diff --git a/WORKSPACE b/WORKSPACE
index 35715d2..d76d359 100644
--- a/WORKSPACE
+++ b/WORKSPACE
@@ -623,13 +623,13 @@
)
# Downloaded from
-# https://developer.arm.com/-/media/Files/downloads/gnu-rm/7-2018q2/gcc-arm-none-eabi-7-2018-q2-update-linux.tar.bz2?revision=bc2c96c0-14b5-4bb4-9f18-bceb4050fee7?product=GNU%20Arm%20Embedded%20Toolchain,64-bit,,Linux,7-2018-q2-update
+# From https://developer.arm.com/downloads/-/arm-gnu-toolchain-downloads
http_archive(
name = "gcc_arm_none_eabi",
build_file = "@//:compilers/gcc_arm_none_eabi.BUILD",
- sha256 = "bb17109f0ee697254a5d4ae6e5e01440e3ea8f0277f2e8169bf95d07c7d5fe69",
- strip_prefix = "gcc-arm-none-eabi-7-2018-q2-update/",
- url = "https://software.frc971.org/Build-Dependencies/gcc-arm-none-eabi-7-2018-q2-update-linux.tar.bz2",
+ sha256 = "6cd1bbc1d9ae57312bcd169ae283153a9572bd6a8e4eeae2fedfbc33b115fdbb",
+ strip_prefix = "arm-gnu-toolchain-13.2.Rel1-x86_64-arm-none-eabi",
+ url = "https://developer.arm.com/-/media/Files/downloads/gnu/13.2.rel1/binrel/arm-gnu-toolchain-13.2.rel1-x86_64-arm-none-eabi.tar.xz",
)
# Java11 JDK.
diff --git a/aos/configuration.cc b/aos/configuration.cc
index 64804e5..6cae0d4 100644
--- a/aos/configuration.cc
+++ b/aos/configuration.cc
@@ -949,10 +949,10 @@
VLOG(1) << "No match for { \"name\": \"" << name << "\", \"type\": \""
<< type << "\" }";
if (original_name != name && !quiet) {
- LOG(WARNING) << "Remapped from {\"name\": \"" << original_name
- << "\", \"type\": \"" << type << "\"}, to {\"name\": \""
- << name << "\", \"type\": \"" << type
- << "\"}, but no channel by that name exists.";
+ VLOG(1) << "Remapped from {\"name\": \"" << original_name
+ << "\", \"type\": \"" << type << "\"}, to {\"name\": \"" << name
+ << "\", \"type\": \"" << type
+ << "\"}, but no channel by that name exists.";
}
return nullptr;
}
diff --git a/aos/containers/BUILD b/aos/containers/BUILD
index ee1bf98..c0b365c 100644
--- a/aos/containers/BUILD
+++ b/aos/containers/BUILD
@@ -42,6 +42,29 @@
)
cc_library(
+ name = "inlined_vector",
+ hdrs = [
+ "inlined_vector.h",
+ ],
+ deps = [
+ "@com_google_absl//absl/container:inlined_vector",
+ ],
+)
+
+cc_test(
+ name = "inlined_vector_test",
+ srcs = [
+ "inlined_vector_test.cc",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ ":inlined_vector",
+ "//aos:realtime",
+ "//aos/testing:googletest",
+ ],
+)
+
+cc_library(
name = "sized_array",
hdrs = [
"sized_array.h",
diff --git a/aos/containers/inlined_vector.h b/aos/containers/inlined_vector.h
new file mode 100644
index 0000000..abd375d
--- /dev/null
+++ b/aos/containers/inlined_vector.h
@@ -0,0 +1,18 @@
+#ifndef AOS_CONTAINERS_INLINED_VECTOR_H_
+#define AOS_CONTAINERS_INLINED_VECTOR_H_
+
+#include <vector>
+
+#include "absl/container/inlined_vector.h"
+
+namespace aos {
+
+template <typename T, size_t N>
+struct InlinedVector : public absl::InlinedVector<T, N> {};
+
+// Specialized for the N == 0 case because absl::InlinedVector doesn't support
+// it for some reason.
+template <typename T>
+struct InlinedVector<T, 0> : public std::vector<T> {};
+} // namespace aos
+#endif // AOS_CONTAINERS_INLINED_VECTOR_H_
diff --git a/aos/containers/inlined_vector_test.cc b/aos/containers/inlined_vector_test.cc
new file mode 100644
index 0000000..7368844
--- /dev/null
+++ b/aos/containers/inlined_vector_test.cc
@@ -0,0 +1,49 @@
+#include "aos/containers/inlined_vector.h"
+
+#include "gtest/gtest.h"
+
+#include "aos/realtime.h"
+
+DECLARE_bool(die_on_malloc);
+
+namespace aos {
+namespace testing {
+
+// Checks that we don't malloc until/unless we need to increase the size of the
+// vector.
+TEST(SizedArrayTest, NoUnnecessaryMalloc) {
+ gflags::FlagSaver flag_saver;
+ FLAGS_die_on_malloc = true;
+ RegisterMallocHook();
+ InlinedVector<int, 5> a;
+ {
+ aos::ScopedRealtime realtime;
+ a.push_back(9);
+ a.push_back(7);
+ a.push_back(1);
+ a.push_back(2);
+ a.push_back(3);
+
+ // And double-check that we can actually construct a new object at realtime.
+ InlinedVector<int, 5> b;
+ }
+// Malloc hooks don't work with asan/msan.
+#if !__has_feature(address_sanitizer) && !__has_feature(memory_sanitizer)
+ EXPECT_DEATH(
+ {
+ aos::ScopedRealtime realtime;
+ a.push_back(4);
+ },
+ "Malloced");
+#endif
+}
+
+// Tests that we can create/define a vector with zero statically allocated
+// elements (the absl::InlinedVector does not allow this for some reason).
+TEST(SizedArrayTest, ZeroLengthVector) {
+ InlinedVector<int, 0> zero;
+ zero.push_back(1);
+ ASSERT_EQ(1, zero[0]);
+}
+} // namespace testing
+} // namespace aos
diff --git a/aos/events/epoll.cc b/aos/events/epoll.cc
index 9a11197..340d0cd 100644
--- a/aos/events/epoll.cc
+++ b/aos/events/epoll.cc
@@ -2,10 +2,13 @@
#include <fcntl.h>
#include <sys/epoll.h>
+#include <sys/socket.h>
+#include <sys/stat.h>
#include <sys/timerfd.h>
#include <unistd.h>
#include <atomic>
+#include <cstring>
#include <vector>
#include "glog/logging.h"
@@ -203,17 +206,47 @@
LOG(FATAL) << "fd " << fd << " not found";
}
+namespace {
+bool IsSocket(int fd) {
+ struct stat st;
+ if (fstat(fd, &st) == -1) {
+ return false;
+ }
+ return static_cast<bool>(S_ISSOCK(st.st_mode));
+}
+
+::std::string GetSocketErrorStr(int fd) {
+ ::std::string error_str;
+ if (IsSocket(fd)) {
+ int error = 0;
+ socklen_t errlen = sizeof(error);
+ if (getsockopt(fd, SOL_SOCKET, SO_ERROR, (void *)&error, &errlen) == 0) {
+ if (error) {
+ error_str = "Socket error: " + ::std::string(strerror(error));
+ }
+ }
+ }
+ return error_str;
+}
+} // namespace
+
void EPoll::InOutEventData::DoCallbacks(uint32_t events) {
if (events & kInEvents) {
- CHECK(in_fn) << ": No handler registered for input events on " << fd;
+ CHECK(in_fn) << ": No handler registered for input events on descriptor "
+ << fd << ". Received events = 0x" << std::hex << events
+ << std::dec;
in_fn();
}
if (events & kOutEvents) {
- CHECK(out_fn) << ": No handler registered for output events on " << fd;
+ CHECK(out_fn) << ": No handler registered for output events on descriptor "
+ << fd << ". Received events = 0x" << std::hex << events
+ << std::dec;
out_fn();
}
if (events & kErrorEvents) {
- CHECK(err_fn) << ": No handler registered for error events on " << fd;
+ CHECK(err_fn) << ": No handler registered for error events on descriptor "
+ << fd << ". Received events = 0x" << std::hex << events
+ << std::dec << ". " << GetSocketErrorStr(fd);
err_fn();
}
}
diff --git a/aos/events/event_loop.h b/aos/events/event_loop.h
index 827dd46..f57053f 100644
--- a/aos/events/event_loop.h
+++ b/aos/events/event_loop.h
@@ -818,6 +818,10 @@
// provided string_view.
void SetVersionString(std::string_view version);
+ std::optional<std::string_view> VersionString() const {
+ return version_string_;
+ }
+
protected:
// Sets the name of the event loop. This is the application name.
virtual void set_name(const std::string_view name) = 0;
diff --git a/aos/events/logging/multinode_logger_test_lib.h b/aos/events/logging/multinode_logger_test_lib.h
index d6d3f60..f5fb4cf 100644
--- a/aos/events/logging/multinode_logger_test_lib.h
+++ b/aos/events/logging/multinode_logger_test_lib.h
@@ -78,13 +78,13 @@
};
constexpr std::string_view kCombinedConfigSha1() {
- return "8f8a0fd505b5cd8ffc553b2af2796232450a2db88c6db974c5e3406b38eecb93";
+ return "32514f3a686e5f8936cc4651e7c81350112f7be8d80dcb8d4afaa29d233c5619";
}
constexpr std::string_view kSplitConfigSha1() {
- return "0a2443f8fe49815de16723c48881f3a78c4e6399273d15da2d77d35588c8cae8";
+ return "416da222c09d83325c6f453591d34c7ef12c12c2dd129ddeea657c4bec61b7fd";
}
constexpr std::string_view kReloggedSplitConfigSha1() {
- return "5e4f4784b890c1fdca7aeeadad8bd0354325f3ed8f11dbc9a81112df87407931";
+ return "3fe428684a38298d3323ef087f44517574da3f07dd84b3740829156d6d870108";
}
LoggerState MakeLoggerState(NodeEventLoopFactory *node,
diff --git a/aos/starter/starter_demo.py b/aos/starter/starter_demo.py
index 89d06a0..12d8775 100755
--- a/aos/starter/starter_demo.py
+++ b/aos/starter/starter_demo.py
@@ -48,5 +48,5 @@
os.makedirs(os.path.dirname(destination), exist_ok=True)
shutil.copy(config, destination)
shutil.copy(config, f"{tmpdir}/aos_config.{suffix}")
- args = [tmpdir + "/starterd"]
+ args = [tmpdir + "/starterd", "--version_string=starter_version"]
subprocess.run(args, check=True, cwd=tmpdir)
diff --git a/aos/starter/starterd.cc b/aos/starter/starterd.cc
index d6caf3f..1ac4514 100644
--- a/aos/starter/starterd.cc
+++ b/aos/starter/starterd.cc
@@ -10,6 +10,8 @@
DEFINE_string(config, "aos_config.json", "File path of aos configuration");
DEFINE_string(user, "",
"Starter runs as though this user ran a SUID binary if set.");
+DEFINE_string(version_string, "",
+ "Version to report for starterd and subprocesses.");
DECLARE_string(shm_base);
DEFINE_bool(purge_shm_base, false,
@@ -58,6 +60,9 @@
const aos::Configuration *config_msg = &config.message();
aos::starter::Starter starter(config_msg);
+ if (!FLAGS_version_string.empty()) {
+ starter.event_loop()->SetVersionString(FLAGS_version_string);
+ }
starter.Run();
diff --git a/aos/starter/subprocess.cc b/aos/starter/subprocess.cc
index b6a20f0..9606adc 100644
--- a/aos/starter/subprocess.cc
+++ b/aos/starter/subprocess.cc
@@ -725,17 +725,22 @@
stop_reason_ = static_cast<aos::starter::LastStopReason>(*read_result);
}
+ const std::string starter_version_string =
+ absl::StrCat("starter version '",
+ event_loop_->VersionString().value_or("unknown"), "'");
switch (status_) {
case aos::starter::State::STARTING: {
if (exit_code_.value() == 0) {
LOG_IF(INFO, quiet_flag_ == QuietLogging::kNo)
<< "Application '" << name_ << "' pid " << pid_
- << " exited with status " << exit_code_.value();
+ << " exited with status " << exit_code_.value() << " and "
+ << starter_version_string;
} else {
LOG_IF(WARNING, quiet_flag_ == QuietLogging::kNo ||
quiet_flag_ == QuietLogging::kNotForDebugging)
<< "Failed to start '" << name_ << "' on pid " << pid_
- << " : Exited with status " << exit_code_.value();
+ << " : Exited with status " << exit_code_.value() << " and "
+ << starter_version_string;
}
if (autorestart()) {
QueueStart();
@@ -753,13 +758,13 @@
} else {
if (quiet_flag_ == QuietLogging::kNo ||
quiet_flag_ == QuietLogging::kNotForDebugging) {
- std::string version_string =
+ const std::string version_string =
latest_timing_report_version_.has_value()
- ? absl::StrCat("'", latest_timing_report_version_.value(),
- "'")
- : "unknown";
+ ? absl::StrCat("version '",
+ latest_timing_report_version_.value(), "'")
+ : starter_version_string;
LOG_IF(WARNING, quiet_flag_ == QuietLogging::kNo)
- << "Application '" << name_ << "' pid " << pid_ << " version "
+ << "Application '" << name_ << "' pid " << pid_ << " "
<< version_string << " exited unexpectedly with status "
<< exit_code_.value();
}
diff --git a/aos/starter/subprocess_test.cc b/aos/starter/subprocess_test.cc
index 12f7f6d..fcc1128 100644
--- a/aos/starter/subprocess_test.cc
+++ b/aos/starter/subprocess_test.cc
@@ -105,6 +105,89 @@
ASSERT_EQ(aos::starter::State::STOPPED, echo_stderr.status());
}
+// Checks that when a child application crashing results in the starter printing
+// out its own version by default.
+TEST_F(SubprocessTest, PrintNoTimingReportVersionString) {
+ const std::string config_file =
+ ::aos::testing::ArtifactPath("aos/events/pingpong_config.json");
+
+ ::testing::internal::CaptureStderr();
+
+ // Set up application without quiet flag active
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig(config_file);
+ aos::ShmEventLoop event_loop(&config.message());
+ event_loop.SetVersionString("version_string");
+ bool observed_stopped = false;
+ Application error_out(
+ "false", "bash", &event_loop,
+ [&observed_stopped, &error_out]() {
+ if (error_out.status() == aos::starter::State::STOPPED) {
+ observed_stopped = true;
+ }
+ },
+ Application::QuietLogging::kNo);
+ error_out.set_args({"-c", "sleep 3; false"});
+
+ error_out.Start();
+ aos::TimerHandler *exit_timer =
+ event_loop.AddTimer([&event_loop]() { event_loop.Exit(); });
+ event_loop.OnRun([&event_loop, exit_timer]() {
+ exit_timer->Schedule(event_loop.monotonic_now() +
+ std::chrono::milliseconds(5000));
+ });
+
+ event_loop.Run();
+
+ // Ensure presence of logs without quiet flag
+ std::string output = ::testing::internal::GetCapturedStderr();
+ std::string expected = "starter version 'version_string'";
+
+ ASSERT_TRUE(output.find(expected) != std::string::npos) << output;
+ EXPECT_TRUE(observed_stopped);
+ EXPECT_EQ(aos::starter::State::STOPPED, error_out.status());
+}
+
+TEST_F(SubprocessTest, PrintFailedToStartVersionString) {
+ const std::string config_file =
+ ::aos::testing::ArtifactPath("aos/events/pingpong_config.json");
+
+ ::testing::internal::CaptureStderr();
+
+ // Set up application without quiet flag active
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig(config_file);
+ aos::ShmEventLoop event_loop(&config.message());
+ event_loop.SetVersionString("version_string");
+ bool observed_stopped = false;
+ Application error_out(
+ "false", "false", &event_loop,
+ [&observed_stopped, &error_out]() {
+ if (error_out.status() == aos::starter::State::STOPPED) {
+ observed_stopped = true;
+ }
+ },
+ Application::QuietLogging::kNo);
+
+ error_out.Start();
+ aos::TimerHandler *exit_timer =
+ event_loop.AddTimer([&event_loop]() { event_loop.Exit(); });
+ event_loop.OnRun([&event_loop, exit_timer]() {
+ exit_timer->Schedule(event_loop.monotonic_now() +
+ std::chrono::milliseconds(1500));
+ });
+
+ event_loop.Run();
+
+ // Ensure presence of logs without quiet flag
+ std::string output = ::testing::internal::GetCapturedStderr();
+ std::string expected = "starter version 'version_string'";
+
+ ASSERT_TRUE(output.find(expected) != std::string::npos) << output;
+ EXPECT_TRUE(observed_stopped);
+ EXPECT_EQ(aos::starter::State::STOPPED, error_out.status());
+}
+
TEST_F(SubprocessTest, UnactiveQuietFlag) {
const std::string config_file =
::aos::testing::ArtifactPath("aos/events/pingpong_config.json");
@@ -142,7 +225,8 @@
std::string expectedRun = "exited unexpectedly with status";
ASSERT_TRUE(output.find(expectedStart) != std::string::npos ||
- output.find(expectedRun) != std::string::npos);
+ output.find(expectedRun) != std::string::npos)
+ << output;
EXPECT_TRUE(observed_stopped);
EXPECT_EQ(aos::starter::State::STOPPED, error_out.status());
}
diff --git a/build_tests/BUILD b/build_tests/BUILD
index c097726..f0ff94d 100644
--- a/build_tests/BUILD
+++ b/build_tests/BUILD
@@ -213,6 +213,7 @@
srcs = [
"upstream_python_test.py",
],
+ target_compatible_with = ["@platforms//os:linux"],
deps = [
"@pip//matplotlib",
"@pip//numpy",
diff --git a/compilers/gcc_arm_none_eabi.BUILD b/compilers/gcc_arm_none_eabi.BUILD
index db2cc77..ba2462a 100644
--- a/compilers/gcc_arm_none_eabi.BUILD
+++ b/compilers/gcc_arm_none_eabi.BUILD
@@ -4,6 +4,7 @@
name = "gcc",
srcs = [
"bin/arm-none-eabi-gcc",
+ ":compiler_pieces",
],
)
@@ -61,6 +62,7 @@
srcs = glob([
"arm-none-eabi/**",
"lib/gcc/arm-none-eabi/**",
+ "libexec/gcc/arm-none-eabi/**",
"include/**",
]),
)
diff --git a/frc971/amd64/build_rootfs.py b/frc971/amd64/build_rootfs.py
index cc0138e..49735c8 100755
--- a/frc971/amd64/build_rootfs.py
+++ b/frc971/amd64/build_rootfs.py
@@ -165,6 +165,10 @@
"--exclude=./usr/bin/X11",
"--exclude=./usr/lib/systemd/system/system-systemd*cryptsetup.slice",
"--exclude=./dev",
+ "--exclude=./usr/include/cub",
+ "--exclude=./usr/include/nv",
+ "--exclude=./usr/include/thrust",
+ "--exclude=./usr/include/cuda",
"-cf",
tarball,
".",
diff --git a/frc971/control_loops/BUILD b/frc971/control_loops/BUILD
index 31b245b..c03a964 100644
--- a/frc971/control_loops/BUILD
+++ b/frc971/control_loops/BUILD
@@ -199,6 +199,13 @@
],
)
+flatbuffer_ts_library(
+ name = "can_falcon_ts_fbs",
+ srcs = [
+ "can_falcon.fbs",
+ ],
+)
+
flatbuffer_cc_library(
name = "can_falcon_fbs",
srcs = [
diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index fdbc1dc..e631799 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -71,6 +71,19 @@
deps = ["//frc971/control_loops:control_loops_ts_fbs"],
)
+flatbuffer_ts_library(
+ name = "drivetrain_position_ts_fbs",
+ srcs = ["drivetrain_position.fbs"],
+ target_compatible_with = ["@platforms//os:linux"],
+)
+
+flatbuffer_ts_library(
+ name = "drivetrain_can_position_ts_fbs",
+ srcs = ["drivetrain_can_position.fbs"],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = ["//frc971/control_loops:can_falcon_ts_fbs"],
+)
+
genrule(
name = "drivetrain_goal_float_fbs_generated",
srcs = ["drivetrain_goal.fbs"],
diff --git a/frc971/control_loops/hybrid_state_feedback_loop.h b/frc971/control_loops/hybrid_state_feedback_loop.h
index 79e7da7..3732266 100644
--- a/frc971/control_loops/hybrid_state_feedback_loop.h
+++ b/frc971/control_loops/hybrid_state_feedback_loop.h
@@ -86,12 +86,12 @@
StateFeedbackHybridPlant(StateFeedbackHybridPlant &&other)
: index_(other.index_) {
- ::std::swap(coefficients_, other.coefficients_);
- X_.swap(other.X_);
- Y_.swap(other.Y_);
- A_.swap(other.A_);
- B_.swap(other.B_);
- DelayedU_.swap(other.DelayedU_);
+ coefficients_ = ::std::move(other.coefficients_);
+ X_ = ::std::move(other.X_);
+ Y_ = ::std::move(other.Y_);
+ A_ = ::std::move(other.A_);
+ B_ = ::std::move(other.B_);
+ DelayedU_ = ::std::move(other.DelayedU_);
}
virtual ~StateFeedbackHybridPlant() {}
@@ -290,12 +290,11 @@
}
HybridKalman(HybridKalman &&other) : index_(other.index_) {
- ::std::swap(coefficients_, other.coefficients_);
-
- X_hat_.swap(other.X_hat_);
- P_.swap(other.P_);
- Q_.swap(other.Q_);
- R_.swap(other.R_);
+ coefficients_ = ::std::move(other.coefficients_);
+ X_hat_ = ::std::move(other.X_hat_);
+ P_ = ::std::move(other.P_);
+ Q_ = ::std::move(other.Q_);
+ R_ = ::std::move(other.R_);
}
// Getters for Q
diff --git a/frc971/orin/BUILD b/frc971/orin/BUILD
index 5675ab4..61d9b8e 100644
--- a/frc971/orin/BUILD
+++ b/frc971/orin/BUILD
@@ -7,3 +7,118 @@
function = "ycbcr",
visibility = ["//visibility:public"],
)
+
+halide_library(
+ name = "ycbcr422",
+ src = "crcv_generator.cc",
+ args = "rows=1088 cols=1456 ystride=1536 cbcrstride=1536",
+ function = "ycbcr422",
+ visibility = ["//visibility:public"],
+)
+
+cc_library(
+ name = "cuda",
+ srcs = [
+ "apriltag.cc",
+ "cuda.cc",
+ "labeling_allegretti_2019_BKE.cc",
+ "points.cc",
+ "threshold.cc",
+ ],
+ hdrs = [
+ "apriltag.h",
+ "cuda.h",
+ "gpu_image.h",
+ "labeling_allegretti_2019_BKE.h",
+ "points.h",
+ "threshold.h",
+ "transform_output_iterator.h",
+ ],
+ copts = [
+ "-Wno-pass-failed",
+ #"-DCUB_DETAIL_DEBUG_ENABLE_LOG=1",
+ #"-DDEBUG=1",
+ ],
+ features = ["cuda"],
+ deps = [
+ "//aos/time",
+ "//third_party/apriltag",
+ "@com_github_google_glog//:glog",
+ "@com_github_nvidia_cccl//:cccl",
+ "@com_github_nvidia_cuco//:cuco",
+ ],
+)
+
+cc_test(
+ name = "cuda_april_tag_test",
+ srcs = [
+ "cuda_april_tag_test.cc",
+ ],
+ data = [
+ "@apriltag_test_bfbs_images",
+ ],
+ features = ["cuda"],
+ deps = [
+ ":cuda",
+ ":ycbcr",
+ "//aos:flatbuffer_merge",
+ "//aos:json_to_flatbuffer",
+ "//aos/testing:googletest",
+ "//aos/testing:path",
+ "//aos/testing:random_seed",
+ "//frc971/vision:vision_fbs",
+ "//third_party:cudart",
+ "//third_party:opencv",
+ "//third_party/apriltag",
+ "//y2023/vision:ThresholdHalide",
+ "//y2023/vision:ToGreyscaleAndDecimateHalide",
+ ],
+)
+
+cc_test(
+ name = "output_iterator_test",
+ srcs = [
+ "output_iterator_test.cc",
+ ],
+ features = ["cuda"],
+ deps = [
+ ":cuda",
+ "//aos/testing:googletest",
+ "//aos/testing:random_seed",
+ "//third_party:cudart",
+ ],
+)
+
+cc_test(
+ name = "points_test",
+ srcs = [
+ "points_test.cc",
+ ],
+ features = ["cuda"],
+ deps = [
+ ":cuda",
+ "//aos/testing:googletest",
+ "//aos/testing:random_seed",
+ "//third_party:cudart",
+ ],
+)
+
+cc_binary(
+ name = "argus_camera",
+ srcs = [
+ "argus_camera.cc",
+ ],
+ target_compatible_with = ["@platforms//cpu:arm64"],
+ deps = [
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ "//aos/util:file",
+ "//frc971/orin:ycbcr",
+ "//frc971/orin:ycbcr422",
+ "//frc971/vision:vision_fbs",
+ "@arm64_debian_sysroot//:argus",
+ "@arm64_debian_sysroot//:egl",
+ "@arm64_debian_sysroot//:eglstream",
+ "@arm64_debian_sysroot//:nvbufsurface",
+ ],
+)
diff --git a/frc971/orin/README.md b/frc971/orin/README.md
new file mode 100644
index 0000000..1b16ba3
--- /dev/null
+++ b/frc971/orin/README.md
@@ -0,0 +1,26 @@
+# Building the rootfs for the Orin NX 8g
+
+Our code targets Debian Bookworm. Debian support for the Orin is poor, but
+yocto support is pretty good. So, rather than try to recreate what Yocto has,
+we instead install the .deb's that Yocto generates for the NVIDIA libraries
+and install them on a bookworm sysroot. To do this:
+
+Check out and follow the instructions on https://github.com/frc971/meta-frc971
+to create a working image for the Orin. You will need this to install.
+
+Then, build a Bookworm image.
+
+```
+./build_rootfs.py
+```
+
+You'll likely have to point that to your yocto build unless your username is
+austin.
+
+From there, copy the resulting image to `./frc971-image-orin-nx-8g.tegraflash.tar.gz`
+
+Then, hold the bootloader switch, reboot the Orin, and then run the install
+script to install it on your orin.
+```
+doflash_frc971.sh
+```
diff --git a/frc971/orin/apriltag.cc b/frc971/orin/apriltag.cc
new file mode 100644
index 0000000..ff8ccec
--- /dev/null
+++ b/frc971/orin/apriltag.cc
@@ -0,0 +1,861 @@
+#include "frc971/orin/apriltag.h"
+
+#include <thrust/iterator/constant_iterator.h>
+#include <thrust/iterator/transform_iterator.h>
+
+#include <cub/device/device_copy.cuh>
+#include <cub/device/device_radix_sort.cuh>
+#include <cub/device/device_reduce.cuh>
+#include <cub/device/device_run_length_encode.cuh>
+#include <cub/device/device_scan.cuh>
+#include <cub/device/device_segmented_sort.cuh>
+#include <cub/device/device_select.cuh>
+#include <cub/iterator/discard_output_iterator.cuh>
+#include <cub/iterator/transform_input_iterator.cuh>
+#include <vector>
+
+#include "glog/logging.h"
+
+#include "aos/time/time.h"
+#include "frc971/orin/labeling_allegretti_2019_BKE.h"
+#include "frc971/orin/threshold.h"
+#include "frc971/orin/transform_output_iterator.h"
+
+namespace frc971 {
+namespace apriltag {
+
+typedef std::chrono::duration<float, std::milli> float_milli;
+
+// Returns true if the QuadBoundaryPoint is nonzero.
+struct NonZero {
+ __host__ __device__ __forceinline__ bool operator()(
+ const QuadBoundaryPoint &a) const {
+ return a.nonzero();
+ }
+};
+
+// Always returns true (only used for scratch space calcs).
+template <typename T>
+struct True {
+ __host__ __device__ __forceinline__ bool operator()(const T &) const {
+ return true;
+ }
+};
+
+// Computes and returns the scratch space needed for DeviceRadixSort::SortKeys
+// of the provided key with the provided number of elements.
+template <typename T>
+static size_t RadixSortScratchSpace(size_t elements) {
+ size_t temp_storage_bytes = 0;
+ QuadBoundaryPointDecomposer decomposer;
+ cub::DeviceRadixSort::SortKeys(nullptr, temp_storage_bytes, (T *)(nullptr),
+ (T *)(nullptr), elements, decomposer);
+ return temp_storage_bytes;
+}
+
+// Computes and returns the scratch space needed for DeviceSelect::If of the
+// provided type Tin, to be written to the provided type Tout, for the provided
+// number of elements. num_markers is the device pointer used to hold the
+// selected number of elements.
+template <typename Tin, typename Tout>
+static size_t DeviceSelectIfScratchSpace(size_t elements, int *num_markers) {
+ size_t temp_storage_bytes = 0;
+ CHECK_CUDA(cub::DeviceSelect::If(nullptr, temp_storage_bytes,
+ (Tin *)(nullptr), (Tout *)(nullptr),
+ num_markers, elements, True<Tin>()));
+ return temp_storage_bytes;
+}
+
+// Always returns the first element (only used for scratch space calcs).
+template <typename T>
+struct CustomFirst {
+ __host__ __device__ __forceinline__ T operator()(const T &a,
+ const T & /*b*/) const {
+ return a;
+ }
+};
+
+// Computes and returns the scratch space needed for DeviceReduce::ReduceByKey
+// of the provided key K and value V with the provided number of elements.
+template <typename K, typename V>
+static size_t DeviceReduceByKeyScratchSpace(size_t elements) {
+ size_t temp_storage_bytes = 0;
+ CHECK_CUDA(cub::DeviceReduce::ReduceByKey(
+ nullptr, temp_storage_bytes, (K *)(nullptr), (K *)(nullptr),
+ (V *)(nullptr), (V *)(nullptr), (size_t *)(nullptr), CustomFirst<V>(),
+ elements));
+ return temp_storage_bytes;
+}
+
+// Computes and returns the scratch space needed for DeviceScan::InclusiveScan
+// of the provided value V with the provided number of elements.
+template <typename V>
+static size_t DeviceScanInclusiveScanScratchSpace(size_t elements) {
+ size_t temp_storage_bytes = 0;
+ CHECK_CUDA(cub::DeviceScan::InclusiveScan(nullptr, temp_storage_bytes,
+ (V *)(nullptr), (V *)(nullptr),
+ CustomFirst<V>(), elements));
+ return temp_storage_bytes;
+}
+
+GpuDetector::GpuDetector(size_t width, size_t height,
+ const apriltag_detector_t *tag_detector)
+ : width_(width),
+ height_(height),
+ tag_detector_(tag_detector),
+ color_image_host_(width * height * 2),
+ color_image_device_(width * height * 2),
+ gray_image_device_(width * height),
+ decimated_image_device_(width / 2 * height / 2),
+ unfiltered_minmax_image_device_((width / 2 / 4 * height / 2 / 4) * 2),
+ minmax_image_device_((width / 2 / 4 * height / 2 / 4) * 2),
+ thresholded_image_device_(width / 2 * height / 2),
+ union_markers_device_(width / 2 * height / 2),
+ union_markers_size_device_(width / 2 * height / 2),
+ union_marker_pair_device_((width / 2 - 2) * (height / 2 - 2) * 4),
+ compressed_union_marker_pair_device_(union_marker_pair_device_.size()),
+ sorted_union_marker_pair_device_(union_marker_pair_device_.size()),
+ extents_device_(union_marker_pair_device_.size()),
+ reduced_dot_blobs_pair_device_(kMaxBlobs),
+ selected_extents_device_(kMaxBlobs),
+ selected_blobs_device_(union_marker_pair_device_.size()),
+ sorted_selected_blobs_device_(selected_blobs_device_.size()),
+ radix_sort_tmpstorage_device_(RadixSortScratchSpace<QuadBoundaryPoint>(
+ sorted_union_marker_pair_device_.size())),
+ temp_storage_compressed_union_marker_pair_device_(
+ DeviceSelectIfScratchSpace<QuadBoundaryPoint, QuadBoundaryPoint>(
+ union_marker_pair_device_.size(),
+ num_compressed_union_marker_pair_device_.get())),
+ temp_storage_bounds_reduce_by_key_device_(
+ DeviceReduceByKeyScratchSpace<uint64_t, MinMaxExtents>(
+ union_marker_pair_device_.size())),
+ temp_storage_dot_product_device_(
+ DeviceReduceByKeyScratchSpace<uint64_t, float>(
+ union_marker_pair_device_.size())),
+ temp_storage_compressed_filtered_blobs_device_(
+ DeviceSelectIfScratchSpace<IndexPoint, IndexPoint>(
+ union_marker_pair_device_.size(),
+ num_selected_blobs_device_.get())),
+ temp_storage_selected_extents_scan_device_(
+ DeviceScanInclusiveScanScratchSpace<
+ cub::KeyValuePair<long, MinMaxExtents>>(kMaxBlobs)) {
+ CHECK_EQ(tag_detector_->quad_decimate, 2);
+ CHECK(!tag_detector_->qtp.deglitch);
+
+ for (int i = 0; i < zarray_size(tag_detector_->tag_families); i++) {
+ apriltag_family_t *family;
+ zarray_get(tag_detector_->tag_families, i, &family);
+ if (family->width_at_border < min_tag_width_) {
+ min_tag_width_ = family->width_at_border;
+ }
+ normal_border_ |= !family->reversed_border;
+ reversed_border_ |= family->reversed_border;
+ }
+ min_tag_width_ /= tag_detector_->quad_decimate;
+ if (min_tag_width_ < 3) {
+ min_tag_width_ = 3;
+ }
+}
+
+GpuDetector::~GpuDetector() {}
+
+// Computes a massive image of 4x QuadBoundaryPoint per pixel with a
+// QuadBoundaryPoint for each pixel pair which crosses a blob boundary.
+template <size_t kBlockWidth, size_t kBlockHeight>
+__global__ void BlobDiff(const uint8_t *thresholded_image,
+ const uint32_t *blobs,
+ const uint32_t *union_markers_size,
+ QuadBoundaryPoint *result, size_t width,
+ size_t height) {
+ __shared__ uint32_t temp_blob_storage[kBlockWidth * kBlockHeight];
+ __shared__ uint8_t temp_image_storage[kBlockWidth * kBlockHeight];
+
+ // We overlap both directions in X, and only one direction in Y.
+ const uint x = blockIdx.x * (blockDim.x - 2) + threadIdx.x;
+ const uint y = blockIdx.y * (blockDim.y - 1) + threadIdx.y;
+
+ // Ignore anything outside the image.
+ if (x >= width || y >= height || y == 0) {
+ return;
+ }
+
+ // Compute the location in the image this threads is responsible for.
+ const uint global_input_index = x + y * width;
+ // And the destination in the temporary storage to save it.
+ const uint thread_linear_index = threadIdx.x + blockDim.x * threadIdx.y;
+
+ // Now, load all the data.
+ const uint32_t rep0 = temp_blob_storage[thread_linear_index] =
+ blobs[global_input_index];
+ const uint8_t v0 = temp_image_storage[thread_linear_index] =
+ thresholded_image[global_input_index];
+
+ // All threads in the block have now gotten this far so the shared memory is
+ // consistent.
+ __syncthreads();
+
+ // We've done our job loading things into memory, this pixel is a boundary
+ // with the upper and left sides, or covered by the next block.
+ if (threadIdx.x == 0) {
+ return;
+ }
+
+ // We need to search both ways in x, this thread doesn't participate further
+ // :(
+ if (threadIdx.x == blockDim.x - 1) {
+ return;
+ }
+
+ if (threadIdx.y == blockDim.y - 1) {
+ return;
+ }
+
+ // This is the last pixel along the lower and right sides, we don't need to
+ // compute it.
+ if (x == width - 1 || y == height - 1) {
+ return;
+ }
+
+ // Place in memory to write the result.
+ const uint global_output_index = (x - 1) + (y - 1) * (width - 2);
+
+ // Short circuit 127's and write an empty point out.
+ if (v0 == 127 || union_markers_size[rep0] < 25) {
+#pragma unroll
+ for (size_t point_offset = 0; point_offset < 4; ++point_offset) {
+ const size_t write_address =
+ (width - 2) * (height - 2) * point_offset + global_output_index;
+ result[write_address] = QuadBoundaryPoint();
+ }
+ return;
+ }
+
+ uint32_t rep1;
+ uint8_t v1;
+
+#define DO_CONN(dx, dy, point_offset) \
+ { \
+ QuadBoundaryPoint cluster_id; \
+ const uint x1 = dx + threadIdx.x; \
+ const uint y1 = dy + threadIdx.y; \
+ const uint thread_linear_index1 = x1 + blockDim.x * y1; \
+ v1 = temp_image_storage[thread_linear_index1]; \
+ rep1 = temp_blob_storage[thread_linear_index1]; \
+ if (v0 + v1 == 255) { \
+ if (union_markers_size[rep1] >= 25) { \
+ if (rep0 < rep1) { \
+ cluster_id.set_rep1(rep1); \
+ cluster_id.set_rep0(rep0); \
+ } else { \
+ cluster_id.set_rep1(rep0); \
+ cluster_id.set_rep0(rep1); \
+ } \
+ cluster_id.set_base_xy(x, y); \
+ cluster_id.set_dxy(point_offset); \
+ cluster_id.set_black_to_white(v1 > v0); \
+ } \
+ } \
+ const size_t write_address = \
+ (width - 2) * (height - 2) * point_offset + global_output_index; \
+ result[write_address] = cluster_id; \
+ }
+
+ // We search the following 4 neighbors.
+ // ________
+ // | x | 0 |
+ // -------------
+ // | 3 | 2 | 1 |
+ // -------------
+ //
+ // If connection 3 has the same IDs as the connection between blocks 0 and 2,
+ // we will have a duplicate entry. Detect and don't add it. This will only
+ // happen if id(x) == id(0) and id(2) == id(3),
+ // or id(x) == id(2) and id(0) ==id(3).
+
+ DO_CONN(1, 0, 0);
+ DO_CONN(1, 1, 1);
+ DO_CONN(0, 1, 2);
+ const uint64_t rep_block_2 = rep1;
+ const uint8_t v1_block_2 = v1;
+
+ const uint left_thread_linear_index1 =
+ threadIdx.x - 1 + blockDim.x * threadIdx.y;
+ const uint8_t v1_block_left = temp_image_storage[left_thread_linear_index1];
+ const uint32_t rep_block_left = temp_blob_storage[left_thread_linear_index1];
+
+ // Do the dedup calculation now.
+ if (v1_block_left != 127 && v1_block_2 != 127 &&
+ v1_block_2 != v1_block_left) {
+ if (x != 1 && union_markers_size[rep_block_left] >= 25 &&
+ union_markers_size[rep_block_2] >= 25) {
+ const size_t write_address =
+ (width - 2) * (height - 2) * 3 + global_output_index;
+ result[write_address] = QuadBoundaryPoint();
+ return;
+ }
+ }
+
+ DO_CONN(-1, 1, 3);
+}
+
+// Masks out just the blob ID pair, rep01.
+struct MaskRep01 {
+ __host__ __device__ __forceinline__ uint64_t
+ operator()(const QuadBoundaryPoint &a) const {
+ return a.rep01();
+ }
+};
+
+// Rewrites a QuadBoundaryPoint to an IndexPoint, adding the angle to the
+// center.
+class RewriteToIndexPoint {
+ public:
+ RewriteToIndexPoint(MinMaxExtents *extents_device, size_t num_extents)
+ : blob_finder_(extents_device, num_extents) {}
+
+ __host__ __device__ __forceinline__ IndexPoint
+ operator()(cub::KeyValuePair<long, QuadBoundaryPoint> pt) const {
+ size_t index = blob_finder_.FindBlobIndex(pt.key);
+ IndexPoint result(index, pt.value.point_bits());
+ return result;
+ }
+
+ BlobExtentsIndexFinder blob_finder_;
+};
+
+// Calculates Theta for a given IndexPoint
+class AddThetaToIndexPoint {
+ public:
+ AddThetaToIndexPoint(MinMaxExtents *extents_device, size_t num_extents)
+ : blob_finder_(extents_device, num_extents) {}
+ __host__ __device__ __forceinline__ IndexPoint operator()(IndexPoint a) {
+ MinMaxExtents extents = blob_finder_.Get(a.blob_index());
+ float theta =
+ (atan2f(a.y() - extents.cy(), a.x() - extents.cx()) + M_PI) * 8e6;
+ long long int theta_int = llrintf(theta);
+
+ a.set_theta(std::max<long long int>(0, theta_int));
+ return a;
+ }
+
+ private:
+ BlobExtentsIndexFinder blob_finder_;
+};
+
+// TODO(austin): Make something which rewrites points on the way back out to
+// memory and adds the slope.
+
+// Transforms aQuadBoundaryPoint into a single point extent for Reduce.
+struct TransformQuadBoundaryPointToMinMaxExtents {
+ __host__ __device__ __forceinline__ MinMaxExtents
+ operator()(cub::KeyValuePair<long, QuadBoundaryPoint> pt) const {
+ MinMaxExtents result;
+ result.min_y = result.max_y = pt.value.y();
+ result.min_x = result.max_x = pt.value.x();
+ result.starting_offset = pt.key;
+ result.count = 1;
+ return result;
+ }
+};
+
+// Reduces 2 extents by tracking the range and updating the offset and count
+// accordingly.
+struct QuadBoundaryPointExtents {
+ __host__ __device__ __forceinline__ MinMaxExtents
+ operator()(const MinMaxExtents &a, const MinMaxExtents &b) const {
+ MinMaxExtents result;
+ result.min_x = std::min(a.min_x, b.min_x);
+ result.max_x = std::max(a.max_x, b.max_x);
+ result.min_y = std::min(a.min_y, b.min_y);
+ result.max_y = std::max(a.max_y, b.max_y);
+ // And the actual start is the first of the points.
+ result.starting_offset = std::min(a.starting_offset, b.starting_offset);
+ // We want to count everything.
+ result.count = a.count + b.count;
+ return result;
+ }
+};
+
+// Computes the dot product of the point vector and gradient for detecting
+// inside-out blobs.
+struct ComputeDotProductTransform {
+ ComputeDotProductTransform(MinMaxExtents *extents_device, size_t num_extents,
+ size_t tag_width, size_t min_cluster_pixels,
+ size_t max_cluster_pixels)
+ : index_finder_(extents_device, num_extents),
+ tag_width_(tag_width),
+ min_cluster_pixels_(std::max<size_t>(24u, min_cluster_pixels)),
+ max_cluster_pixels_(max_cluster_pixels) {}
+
+ __host__ __device__ __forceinline__ float operator()(
+ cub::KeyValuePair<long, QuadBoundaryPoint> a) const {
+ const size_t y = a.value.y();
+ const size_t x = a.value.x();
+
+ // Binary search for the index.
+ const size_t index = index_finder_.FindBlobIndex(a.key);
+
+ // Don't bother to compute the dot product for anything we will ignore in
+ // SelectBlobs below.
+ //
+ // TODO(austin): Figure out how to dedup with SelectBlobs below.
+ const MinMaxExtents extents = index_finder_.Get(index);
+ if (extents.count < min_cluster_pixels_) {
+ return 0;
+ }
+ if (extents.count > max_cluster_pixels_) {
+ return 0;
+ }
+
+ // Area must also be reasonable.
+ if ((extents.max_x - extents.min_x) * (extents.max_y - extents.min_y) <
+ tag_width_) {
+ return 0;
+ }
+
+ const float dx = static_cast<float>(x) - extents.cx();
+ const float dy = static_cast<float>(y) - extents.cy();
+
+ return dx * a.value.gx() + dy * a.value.gy();
+ }
+
+ BlobExtentsIndexFinder index_finder_;
+
+ size_t tag_width_;
+ size_t min_cluster_pixels_;
+ size_t max_cluster_pixels_;
+};
+
+// Selects blobs which are big enough, not too big, and have the right color in
+// the middle.
+class SelectBlobs {
+ public:
+ SelectBlobs(const MinMaxExtents *extents_device, const float *dot_products,
+ size_t tag_width, bool reversed_border, bool normal_border,
+ size_t min_cluster_pixels, size_t max_cluster_pixels)
+ : extents_device_(extents_device),
+ dot_products_(dot_products),
+ tag_width_(tag_width),
+ reversed_border_(reversed_border),
+ normal_border_(normal_border),
+ min_cluster_pixels_(std::max<size_t>(24u, min_cluster_pixels)),
+ max_cluster_pixels_(max_cluster_pixels) {}
+
+ // Returns true if the blob passes the size and dot product checks and is
+ // worth further consideration.
+ __host__ __device__ __forceinline__ bool operator()(
+ const size_t blob_index, MinMaxExtents extents) const {
+ if (extents.count < min_cluster_pixels_) {
+ return false;
+ }
+ if (extents.count > max_cluster_pixels_) {
+ return false;
+ }
+
+ // Area must also be reasonable.
+ if ((extents.max_x - extents.min_x) * (extents.max_y - extents.min_y) <
+ tag_width_) {
+ return false;
+ }
+
+ // And the right side must be inside.
+ const bool quad_reversed_border = dot_products_[blob_index] < 0;
+ if (!reversed_border_ && quad_reversed_border) {
+ return false;
+ }
+ if (!normal_border_ && !quad_reversed_border) {
+ return false;
+ }
+
+ return true;
+ }
+
+ __host__ __device__ __forceinline__ bool operator()(
+ const IndexPoint &a) const {
+ bool result = (*this)(a.blob_index(), extents_device_[a.blob_index()]);
+
+ return result;
+ }
+
+ const MinMaxExtents *extents_device_;
+ const float *dot_products_;
+ size_t tag_width_;
+
+ bool reversed_border_;
+ bool normal_border_;
+ size_t min_cluster_pixels_;
+ size_t max_cluster_pixels_;
+};
+
+// Class to zero out the count (and clear the starting offset) for each extents
+// which is going to be filtered out. Used in conjunction with SumPoints to
+// update zero sized blobs.
+struct TransformZeroFilteredBlobSizes {
+ public:
+ TransformZeroFilteredBlobSizes(const float *dot_products, size_t tag_width,
+ bool reversed_border, bool normal_border,
+ size_t min_cluster_pixels,
+ size_t max_cluster_pixels)
+ : select_blobs_(nullptr, dot_products, tag_width, reversed_border,
+ normal_border, min_cluster_pixels, max_cluster_pixels) {}
+
+ __host__ __device__ __forceinline__ cub::KeyValuePair<long, MinMaxExtents>
+ operator()(cub::KeyValuePair<long, MinMaxExtents> pt) const {
+ pt.value.count *= select_blobs_(pt.key, pt.value);
+ pt.value.starting_offset = 0;
+ return pt;
+ }
+
+ private:
+ SelectBlobs select_blobs_;
+};
+
+// Class to implement a custom Scan operator which passes through the previous
+// min/max/etc, but re-sums count into starting_offset. This lets us collapse
+// out regions which don't pass the minimum filters.
+struct SumPoints {
+ __host__ __device__ __forceinline__ cub::KeyValuePair<long, MinMaxExtents>
+ operator()(const cub::KeyValuePair<long, MinMaxExtents> &a,
+ const cub::KeyValuePair<long, MinMaxExtents> &b) const {
+ cub::KeyValuePair<long, MinMaxExtents> result;
+ if (a.key < b.key) {
+ result.value.min_x = b.value.min_x;
+ result.value.min_y = b.value.min_y;
+ result.value.max_x = b.value.max_x;
+ result.value.max_y = b.value.max_y;
+ result.value.count = b.value.count;
+ result.key = b.key;
+ } else {
+ result.value.min_x = a.value.min_x;
+ result.value.min_y = a.value.min_y;
+ result.value.max_x = a.value.max_x;
+ result.value.max_y = a.value.max_y;
+ result.value.count = a.value.count;
+ result.key = a.key;
+ }
+
+ result.value.starting_offset = b.value.starting_offset + b.value.count;
+ return result;
+ }
+};
+
+void GpuDetector::Detect(const uint8_t *image) {
+ color_image_host_.MemcpyFrom(image);
+ start_.Record(&stream_);
+ color_image_device_.MemcpyAsyncFrom(&color_image_host_, &stream_);
+ after_image_memcpy_to_device_.Record(&stream_);
+
+ // Threshold the image.
+ CudaToGreyscaleAndDecimateHalide(
+ color_image_device_.get(), gray_image_device_.get(),
+ decimated_image_device_.get(), unfiltered_minmax_image_device_.get(),
+ minmax_image_device_.get(), thresholded_image_device_.get(), width_,
+ height_, tag_detector_->qtp.min_white_black_diff, &stream_);
+ after_threshold_.Record(&stream_);
+
+ union_markers_size_device_.MemsetAsync(0u, &stream_);
+ after_memset_.Record(&stream_);
+
+ // Unionfind the image.
+ LabelImage(ToGpuImage(thresholded_image_device_),
+ ToGpuImage(union_markers_device_),
+ ToGpuImage(union_markers_size_device_), stream_.get());
+
+ after_unionfinding_.Record(&stream_);
+
+ CHECK((width_ % 8) == 0);
+ CHECK((height_ % 8) == 0);
+
+ size_t decimated_width = width_ / 2;
+ size_t decimated_height = height_ / 2;
+
+ // TODO(austin): Tune for the global shutter camera.
+ // 1280 -> 2 * 128 * 5
+ // 720 -> 2 * 8 * 5 * 9
+
+ // Compute the unfiltered list of blob pairs and points.
+ {
+ constexpr size_t kBlockWidth = 32;
+ constexpr size_t kBlockHeight = 16;
+ dim3 threads(kBlockWidth, kBlockHeight, 1);
+ // Overlap 1 on each side in x, and 1 in y.
+ dim3 blocks((decimated_width + threads.x - 3) / (threads.x - 2),
+ (decimated_height + threads.y - 2) / (threads.y - 1), 1);
+
+ // Make sure we fit in our mask.
+ CHECK_LT(width_ * height_, static_cast<size_t>(1 << 22));
+
+ BlobDiff<kBlockWidth, kBlockHeight><<<blocks, threads, 0, stream_.get()>>>(
+ thresholded_image_device_.get(), union_markers_device_.get(),
+ union_markers_size_device_.get(), union_marker_pair_device_.get(),
+ decimated_width, decimated_height);
+ MaybeCheckAndSynchronize();
+ }
+
+ // TODO(austin): Can I do the first step of the zero removal in the BlobDiff
+ // kernel?
+
+ after_diff_.Record(&stream_);
+
+ {
+ // Remove empty points which aren't to be considered before sorting to speed
+ // things up.
+ size_t temp_storage_bytes =
+ temp_storage_compressed_union_marker_pair_device_.size();
+ NonZero nz;
+ CHECK_CUDA(cub::DeviceSelect::If(
+ temp_storage_compressed_union_marker_pair_device_.get(),
+ temp_storage_bytes, union_marker_pair_device_.get(),
+ compressed_union_marker_pair_device_.get(),
+ num_compressed_union_marker_pair_device_.get(),
+ union_marker_pair_device_.size(), nz, stream_.get()));
+
+ MaybeCheckAndSynchronize();
+ }
+
+ after_compact_.Record(&stream_);
+
+ int num_compressed_union_marker_pair_host;
+ {
+ num_compressed_union_marker_pair_device_.MemcpyTo(
+ &num_compressed_union_marker_pair_host);
+ CHECK_LT(static_cast<size_t>(num_compressed_union_marker_pair_host),
+ union_marker_pair_device_.size());
+
+ // Now, sort just the keys to group like points.
+ size_t temp_storage_bytes = radix_sort_tmpstorage_device_.size();
+ QuadBoundaryPointDecomposer decomposer;
+ CHECK_CUDA(cub::DeviceRadixSort::SortKeys(
+ radix_sort_tmpstorage_device_.get(), temp_storage_bytes,
+ compressed_union_marker_pair_device_.get(),
+ sorted_union_marker_pair_device_.get(),
+ num_compressed_union_marker_pair_host, decomposer,
+ QuadBoundaryPoint::kRepEndBit, QuadBoundaryPoint::kBitsInKey,
+ stream_.get()));
+
+ MaybeCheckAndSynchronize();
+ }
+
+ after_sort_.Record(&stream_);
+
+ size_t num_quads_host = 0;
+ {
+ // Our next step is to compute the extents of each blob so we can filter
+ // blobs.
+ cub::ArgIndexInputIterator<QuadBoundaryPoint *> value_index_input_iterator(
+ sorted_union_marker_pair_device_.get());
+ TransformQuadBoundaryPointToMinMaxExtents min_max;
+ cub::TransformInputIterator<MinMaxExtents,
+ TransformQuadBoundaryPointToMinMaxExtents,
+ cub::ArgIndexInputIterator<QuadBoundaryPoint *>>
+ value_input_iterator(value_index_input_iterator, min_max);
+
+ // Don't care about the output keys...
+ cub::DiscardOutputIterator<uint64_t> key_discard_iterator;
+
+ // Provide a mask to detect keys by rep01()
+ MaskRep01 mask;
+ cub::TransformInputIterator<uint64_t, MaskRep01, QuadBoundaryPoint *>
+ key_input_iterator(sorted_union_marker_pair_device_.get(), mask);
+
+ // Reduction operator.
+ QuadBoundaryPointExtents reduce;
+
+ size_t temp_storage_bytes =
+ temp_storage_bounds_reduce_by_key_device_.size();
+ cub::DeviceReduce::ReduceByKey(
+ temp_storage_bounds_reduce_by_key_device_.get(), temp_storage_bytes,
+ key_input_iterator, key_discard_iterator, value_input_iterator,
+ extents_device_.get(), num_quads_device_.get(), reduce,
+ num_compressed_union_marker_pair_host, stream_.get());
+ after_bounds_.Record(&stream_);
+
+ num_quads_device_.MemcpyTo(&num_quads_host);
+ }
+ CHECK_LE(num_quads_host, reduced_dot_blobs_pair_device_.size());
+
+ // Longest april tag will be the full perimeter of the image. Each point
+ // results in 2 neighbor points, 1 straight, and one at 45 degrees. But, we
+ // are in decimated space here, and width_ and height_ are in full image
+ // space. And there are 2 sides to each rectangle...
+ //
+ // Aprilrobotics has a *3 instead of a *2 here since they have duplicated
+ // points in their list at this stage.
+ const size_t max_april_tag_perimeter = 2 * (width_ + height_);
+
+ {
+ // Now that we have the extents, compute the dot product used to detect
+ // which blobs are inside out or right side out.
+
+ // Don't care about these quantities since the previous ReduceByKey will
+ // ahve computed them too.
+ cub::DiscardOutputIterator<size_t> length_discard_iterator;
+ cub::DiscardOutputIterator<uint64_t> key_discard_iterator;
+
+ size_t dot_reduce_temp_storage_bytes =
+ temp_storage_dot_product_device_.size();
+
+ // Provide a mask to detect keys by rep01()
+ MaskRep01 mask;
+ cub::TransformInputIterator<uint64_t, MaskRep01, QuadBoundaryPoint *>
+ key_iterator(sorted_union_marker_pair_device_.get(), mask);
+
+ // Create a transform iterator which calculates the dot product for each
+ // point individually.
+ cub::ArgIndexInputIterator<QuadBoundaryPoint *> value_index_input_iterator(
+ sorted_union_marker_pair_device_.get());
+ ComputeDotProductTransform dot_operator(
+ extents_device_.get(), num_quads_host, min_tag_width_,
+ tag_detector_->qtp.min_cluster_pixels, max_april_tag_perimeter);
+ cub::TransformInputIterator<float, ComputeDotProductTransform,
+ cub::ArgIndexInputIterator<QuadBoundaryPoint *>>
+ value_iterator(value_index_input_iterator, dot_operator);
+
+ // And now sum per key.
+ CHECK_CUDA(cub::DeviceReduce::ReduceByKey(
+ temp_storage_dot_product_device_.get(), dot_reduce_temp_storage_bytes,
+ key_iterator, key_discard_iterator, value_iterator,
+ reduced_dot_blobs_pair_device_.get(), length_discard_iterator,
+ cub::Sum(), num_compressed_union_marker_pair_host, stream_.get()));
+
+ MaybeCheckAndSynchronize();
+ }
+
+ after_dot_.Record(&stream_);
+
+ {
+ // Now that we have the dot products, we need to rewrite the extents for the
+ // post-thresholded world so we can find the start and end address of blobs
+ // for fitting lines.
+ //
+ // Clear the size of non-passing extents and the starting offset of all
+ // extents.
+ cub::ArgIndexInputIterator<MinMaxExtents *> value_index_input_iterator(
+ extents_device_.get());
+ TransformZeroFilteredBlobSizes rewrite(
+ reduced_dot_blobs_pair_device_.get(), min_tag_width_, reversed_border_,
+ normal_border_, tag_detector_->qtp.min_cluster_pixels,
+ max_april_tag_perimeter);
+ cub::TransformInputIterator<cub::KeyValuePair<long, MinMaxExtents>,
+ TransformZeroFilteredBlobSizes,
+ cub::ArgIndexInputIterator<MinMaxExtents *>>
+ input_iterator(value_index_input_iterator, rewrite);
+
+ // Sum the counts of everything before us, and update the offset.
+ SumPoints sum_points;
+
+ // TODO(justin): Rip the key off when writing.
+
+ // Rewrite the extents to have the starting offset and count match the
+ // post-selected values.
+ size_t temp_storage_bytes =
+ temp_storage_selected_extents_scan_device_.size();
+ CHECK_CUDA(cub::DeviceScan::InclusiveScan(
+ temp_storage_selected_extents_scan_device_.get(), temp_storage_bytes,
+ input_iterator, selected_extents_device_.get(), sum_points,
+ num_quads_host));
+
+ MaybeCheckAndSynchronize();
+ }
+
+ after_transform_extents_.Record(&stream_);
+
+ int num_selected_blobs_host;
+ {
+ // Now, copy over all points which pass our thresholds.
+ cub::ArgIndexInputIterator<QuadBoundaryPoint *> value_index_input_iterator(
+ sorted_union_marker_pair_device_.get());
+ RewriteToIndexPoint rewrite(extents_device_.get(), num_quads_host);
+
+ cub::TransformInputIterator<IndexPoint, RewriteToIndexPoint,
+ cub::ArgIndexInputIterator<QuadBoundaryPoint *>>
+ input_iterator(value_index_input_iterator, rewrite);
+
+ AddThetaToIndexPoint add_theta(extents_device_.get(), num_quads_host);
+
+ TransformOutputIterator<IndexPoint, IndexPoint, AddThetaToIndexPoint>
+ output_iterator(selected_blobs_device_.get(), add_theta);
+
+ SelectBlobs select_blobs(
+ extents_device_.get(), reduced_dot_blobs_pair_device_.get(),
+ min_tag_width_, reversed_border_, normal_border_,
+ tag_detector_->qtp.min_cluster_pixels, max_april_tag_perimeter);
+
+ size_t temp_storage_bytes =
+ temp_storage_compressed_filtered_blobs_device_.size();
+
+ CHECK_CUDA(cub::DeviceSelect::If(
+ temp_storage_compressed_filtered_blobs_device_.get(),
+ temp_storage_bytes, input_iterator, output_iterator,
+ num_selected_blobs_device_.get(), num_compressed_union_marker_pair_host,
+ select_blobs, stream_.get()));
+ MaybeCheckAndSynchronize();
+
+ num_selected_blobs_device_.MemcpyTo(&num_selected_blobs_host);
+ }
+
+ after_filter_.Record(&stream_);
+
+ {
+ // Sort based on the angle.
+ size_t temp_storage_bytes = radix_sort_tmpstorage_device_.size();
+ QuadIndexPointDecomposer decomposer;
+
+ CHECK_CUDA(cub::DeviceRadixSort::SortKeys(
+ radix_sort_tmpstorage_device_.get(), temp_storage_bytes,
+ selected_blobs_device_.get(), sorted_selected_blobs_device_.get(),
+ num_selected_blobs_host, decomposer, IndexPoint::kRepEndBit,
+ IndexPoint::kBitsInKey, stream_.get()));
+
+ MaybeCheckAndSynchronize();
+ }
+
+ after_filtered_sort_.Record(&stream_);
+
+ // Report out how long things took.
+ LOG(INFO) << "Found " << num_compressed_union_marker_pair_host << " items";
+ LOG(INFO) << "Selected " << num_selected_blobs_host << " blobs";
+
+ CudaEvent *previous_event = &start_;
+ for (auto name_event : std::vector<std::tuple<std::string_view, CudaEvent &>>{
+ {"Memcpy", after_image_memcpy_to_device_},
+ {"Threshold", after_threshold_},
+ {"Memset", after_memset_},
+ {"Unionfinding", after_unionfinding_},
+ {"Diff", after_diff_},
+ {"Compact", after_compact_},
+ {"Sort", after_sort_},
+ {"Bounds", after_bounds_},
+ {"Dot product", after_dot_},
+ {"Transform Extents", after_transform_extents_},
+ {"Filter by dot product", after_filter_},
+ {"Filtered sort", after_filtered_sort_},
+ }) {
+ std::get<1>(name_event).Synchronize();
+ LOG(INFO) << std::get<0>(name_event) << " "
+ << float_milli(
+ std::get<1>(name_event).ElapsedTime(*previous_event))
+ .count()
+ << "ms";
+ previous_event = &std::get<1>(name_event);
+ }
+
+ LOG(INFO) << "Overall "
+ << float_milli(previous_event->ElapsedTime(start_)).count() << "ms";
+
+ // Average. Skip the first one as the kernel is warming up and is slower.
+ if (!first_) {
+ ++execution_count_;
+ execution_duration_ += previous_event->ElapsedTime(start_);
+ LOG(INFO) << "Average overall "
+ << float_milli(execution_duration_ / execution_count_).count()
+ << "ms";
+ }
+
+ LOG(INFO) << "Found compressed runs: " << num_quads_host;
+
+ first_ = false;
+}
+
+} // namespace apriltag
+} // namespace frc971
diff --git a/frc971/orin/apriltag.h b/frc971/orin/apriltag.h
new file mode 100644
index 0000000..068c0c7
--- /dev/null
+++ b/frc971/orin/apriltag.h
@@ -0,0 +1,280 @@
+#ifndef FRC971_ORIN_APRILTAG_H_
+#define FRC971_ORIN_APRILTAG_H_
+
+#include <cub/iterator/transform_input_iterator.cuh>
+
+#include "third_party/apriltag/apriltag.h"
+
+#include "cuda_runtime.h"
+#include "device_launch_parameters.h"
+#include "frc971/orin/cuda.h"
+#include "frc971/orin/gpu_image.h"
+#include "frc971/orin/points.h"
+
+namespace frc971 {
+namespace apriltag {
+
+// Class to hold the extents of a blob of points.
+struct MinMaxExtents {
+ // Min and max coordinates (in non-decimated coordinates)
+ uint16_t min_x;
+ uint16_t min_y;
+ uint16_t max_x;
+ uint16_t max_y;
+
+ // The starting offset of this blob of points in the vector holding all the
+ // points.
+ uint32_t starting_offset;
+
+ // The number of points in the blob.
+ uint32_t count;
+
+ // Center location of the blob using the aprilrobotics algorithm.
+ __host__ __device__ float cx() const {
+ return (min_x + max_x) * 0.5f + 0.05118;
+ }
+ __host__ __device__ float cy() const {
+ return (min_y + max_y) * 0.5f + -0.028581;
+ }
+};
+
+static_assert(sizeof(MinMaxExtents) == 16, "MinMaxExtents didn't pack right.");
+
+// Class to find the blob index of a point in a point vector.
+class BlobExtentsIndexFinder {
+ public:
+ BlobExtentsIndexFinder(const MinMaxExtents *extents_device,
+ size_t num_extents)
+ : extents_device_(extents_device), num_extents_(num_extents) {}
+
+ __host__ __device__ size_t FindBlobIndex(size_t point_index) const {
+ // Do a binary search for the blob which has the point in it's
+ // starting_offset range.
+ size_t min = 0;
+ size_t max = num_extents_;
+ while (true) {
+ if (min + 1 == max) {
+ return min;
+ }
+
+ size_t average = min + (max - min) / 2;
+
+ if (average < num_extents_ && extents_device_[average].starting_offset <=
+ static_cast<size_t>(point_index)) {
+ min = average;
+ } else {
+ max = average;
+ }
+ }
+ }
+
+ // Returns the extents for a blob index.
+ __host__ __device__ MinMaxExtents Get(size_t index) const {
+ return extents_device_[index];
+ }
+
+ private:
+ const MinMaxExtents *extents_device_;
+ size_t num_extents_;
+
+ // TODO(austin): Cache the last one?
+};
+
+// GPU based april tag detector.
+class GpuDetector {
+ public:
+ // The number of blobs we will consider when counting april tags.
+ static constexpr size_t kMaxBlobs = IndexPoint::kMaxBlobs;
+
+ // Constructs a detector, reserving space for detecting tags of the provided
+ // with and height, using the provided detector options.
+ GpuDetector(size_t width, size_t height,
+ const apriltag_detector_t *tag_detector);
+
+ virtual ~GpuDetector();
+
+ // Detects april tags in the provided image.
+ void Detect(const uint8_t *image);
+
+ // Debug methods to expose internal state for testing.
+ void CopyGrayTo(uint8_t *output) { gray_image_device_.MemcpyTo(output); }
+ void CopyDecimatedTo(uint8_t *output) {
+ decimated_image_device_.MemcpyTo(output);
+ }
+ void CopyThresholdedTo(uint8_t *output) {
+ thresholded_image_device_.MemcpyTo(output);
+ }
+ void CopyUnionMarkersTo(uint32_t *output) {
+ union_markers_device_.MemcpyTo(output);
+ }
+
+ void CopyUnionMarkerPairTo(QuadBoundaryPoint *output) {
+ union_marker_pair_device_.MemcpyTo(output);
+ }
+
+ void CopyCompressedUnionMarkerPairTo(QuadBoundaryPoint *output) {
+ compressed_union_marker_pair_device_.MemcpyTo(output);
+ }
+
+ std::vector<QuadBoundaryPoint> CopySortedUnionMarkerPair() {
+ std::vector<QuadBoundaryPoint> result;
+ int size = NumCompressedUnionMarkerPairs();
+ result.resize(size);
+ sorted_union_marker_pair_device_.MemcpyTo(result.data(), size);
+ return result;
+ }
+
+ int NumCompressedUnionMarkerPairs() {
+ return num_compressed_union_marker_pair_device_.Copy()[0];
+ }
+
+ void CopyUnionMarkersSizeTo(uint32_t *output) {
+ union_markers_size_device_.MemcpyTo(output);
+ }
+
+ int NumQuads() const { return num_quads_device_.Copy()[0]; }
+
+ std::vector<MinMaxExtents> CopyExtents() {
+ return extents_device_.Copy(NumQuads());
+ }
+
+ std::vector<float> CopyReducedDotBlobs() {
+ return reduced_dot_blobs_pair_device_.Copy(NumQuads());
+ }
+
+ std::vector<cub::KeyValuePair<long, MinMaxExtents>> CopySelectedExtents() {
+ return selected_extents_device_.Copy(NumQuads());
+ }
+
+ int NumSelectedPairs() const { return num_selected_blobs_device_.Copy()[0]; }
+
+ std::vector<IndexPoint> CopySelectedBlobs() {
+ return selected_blobs_device_.Copy(NumSelectedPairs());
+ }
+
+ std::vector<IndexPoint> CopySortedSelectedBlobs() {
+ return sorted_selected_blobs_device_.Copy(NumSelectedPairs());
+ }
+
+ private:
+ // Creates a GPU image wrapped around the provided memory.
+ template <typename T>
+ GpuImage<T> ToGpuImage(GpuMemory<T> &memory) {
+ if (memory.size() == width_ * height_) {
+ return GpuImage<T>{
+ .data = memory.get(),
+ .rows = height_,
+ .cols = width_,
+ .step = width_,
+ };
+ } else if (memory.size() == width_ * height_ / 4) {
+ return GpuImage<T>{
+ .data = memory.get(),
+ .rows = height_ / 2,
+ .cols = width_ / 2,
+ .step = width_ / 2,
+ };
+ } else {
+ LOG(FATAL) << "Unknown image shape";
+ }
+ }
+
+ // Size of the image.
+ const size_t width_;
+ const size_t height_;
+
+ // Detector parameters.
+ const apriltag_detector_t *tag_detector_;
+
+ // Stream to operate on.
+ CudaStream stream_;
+
+ // Events for each of the steps for timing.
+ CudaEvent start_;
+ CudaEvent after_image_memcpy_to_device_;
+ CudaEvent after_threshold_;
+ CudaEvent after_memset_;
+ CudaEvent after_unionfinding_;
+ CudaEvent after_diff_;
+ CudaEvent after_compact_;
+ CudaEvent after_sort_;
+ CudaEvent after_bounds_;
+ CudaEvent after_dot_;
+ CudaEvent after_transform_extents_;
+ CudaEvent after_filter_;
+ CudaEvent after_filtered_sort_;
+
+ // TODO(austin): Remove this...
+ HostMemory<uint8_t> color_image_host_;
+
+ // Starting color image.
+ GpuMemory<uint8_t> color_image_device_;
+ // Full size gray scale image.
+ GpuMemory<uint8_t> gray_image_device_;
+ // Half resolution, gray, decimated image.
+ GpuMemory<uint8_t> decimated_image_device_;
+ // Intermediates for thresholding.
+ GpuMemory<uint8_t> unfiltered_minmax_image_device_;
+ GpuMemory<uint8_t> minmax_image_device_;
+ GpuMemory<uint8_t> thresholded_image_device_;
+
+ // The union markers for each pixel.
+ GpuMemory<uint32_t> union_markers_device_;
+ // The size of each blob. The blob size is stored at the index of the stored
+ // union marker id in union_markers_device_ aboe.
+ GpuMemory<uint32_t> union_markers_size_device_;
+
+ // Full list of boundary points, densly stored but mostly zero.
+ GpuMemory<QuadBoundaryPoint> union_marker_pair_device_;
+ // Unsorted list of points with 0's removed.
+ GpuMemory<QuadBoundaryPoint> compressed_union_marker_pair_device_;
+ // Blob representation sorted list of points.
+ GpuMemory<QuadBoundaryPoint> sorted_union_marker_pair_device_;
+ // Number of compressed points.
+ GpuMemory<int> num_compressed_union_marker_pair_device_{
+ /* allocate 1 integer...*/ 1};
+
+ // Number of unique blob IDs.
+ GpuMemory<size_t> num_quads_device_{/* allocate 1 integer...*/ 1};
+ // Bounds per blob, one blob per ID.
+ GpuMemory<MinMaxExtents> extents_device_;
+ // Sum of the dot products between the vector from center to each point, with
+ // the gradient at each point.
+ GpuMemory<float> reduced_dot_blobs_pair_device_;
+ // Extents of all the blobs under consideration.
+ GpuMemory<cub::KeyValuePair<long, MinMaxExtents>> selected_extents_device_;
+
+ // Number of keys in selected_blobs_device_.
+ GpuMemory<int> num_selected_blobs_device_{/* allocate 1 integer...*/ 1};
+
+ // Compacted blobs which pass our threshold.
+ GpuMemory<IndexPoint> selected_blobs_device_;
+ // Sorted list of those points.
+ GpuMemory<IndexPoint> sorted_selected_blobs_device_;
+
+ // Temporary storage for each of the steps.
+ // TODO(austin): Can we combine these and just use the max?
+ GpuMemory<uint32_t> radix_sort_tmpstorage_device_;
+ GpuMemory<uint8_t> temp_storage_compressed_union_marker_pair_device_;
+ GpuMemory<uint8_t> temp_storage_bounds_reduce_by_key_device_;
+ GpuMemory<uint8_t> temp_storage_dot_product_device_;
+ GpuMemory<uint8_t> temp_storage_compressed_filtered_blobs_device_;
+ GpuMemory<uint8_t> temp_storage_selected_extents_scan_device_;
+
+ // Cumulative duration of april tag detection.
+ std::chrono::nanoseconds execution_duration_{0};
+ // Number of detections.
+ size_t execution_count_ = 0;
+ // True if this is the first detection.
+ bool first_ = true;
+
+ // Cached quantities used for tag filtering.
+ bool normal_border_ = false;
+ bool reversed_border_ = false;
+ int min_tag_width_ = 1000000;
+};
+
+} // namespace apriltag
+} // namespace frc971
+
+#endif // FRC971_ORIN_APRILTAG_H_
diff --git a/frc971/orin/argus_camera.cc b/frc971/orin/argus_camera.cc
new file mode 100644
index 0000000..ca3d34a
--- /dev/null
+++ b/frc971/orin/argus_camera.cc
@@ -0,0 +1,625 @@
+#include <chrono>
+#include <filesystem>
+#include <thread>
+
+#include "glog/logging.h"
+
+#include "Argus/Argus.h"
+#include "Argus/EGLStream.h"
+#include "Argus/Types.h"
+#include "Argus/utils/Error.h"
+#include "EGLStream/FrameConsumer.h"
+#include "EGLStream/Image.h"
+#include "EGLStream/NV/ImageNativeBuffer.h"
+#include "HalideBuffer.h"
+#include "HalideRuntime.h"
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "aos/time/time.h"
+#include "aos/util/file.h"
+#include "frc971/orin/ycbcr.h"
+#include "frc971/orin/ycbcr422.h"
+#include "frc971/vision/vision_generated.h"
+#include "nvbufsurface.h"
+
+DEFINE_string(config, "aos_config.json", "Path to the config file to use.");
+
+DEFINE_int32(colorformat, NVBUF_COLOR_FORMAT_NV16,
+ "Mode to use. Don't change unless you know what you are doing.");
+DEFINE_int32(camera, 0, "Camera number");
+DEFINE_int32(mode, 0, "Mode number to use.");
+DEFINE_int32(exposure, 200000, "Exposure number to use.");
+DEFINE_int32(gain, 5, "gain number to use.");
+DEFINE_int32(width, 1456, "Image width");
+DEFINE_int32(height, 1088, "Image height");
+DEFINE_double(rgain, 1.0, "R gain");
+DEFINE_double(g1gain, 1.0, "G gain");
+DEFINE_double(g2gain, 1.0, "G gain");
+DEFINE_double(bgain, 1.0, "B gain");
+DEFINE_string(channel, "/camera", "Channel name for the image.");
+
+namespace frc971 {
+
+namespace chrono = std::chrono;
+
+// Converts a multiplanar 422 image into a single plane 422 image at the
+// provided memory location sutable for putting in a flatbuffer.
+void YCbCr422(NvBufSurface *nvbuf_surf, uint8_t *data_pointer) {
+ CHECK_EQ(nvbuf_surf->surfaceList->planeParams.width[0],
+ nvbuf_surf->surfaceList->planeParams.width[1] * 2);
+ CHECK_EQ(nvbuf_surf->surfaceList->planeParams.height[0],
+ nvbuf_surf->surfaceList->planeParams.height[1]);
+ CHECK_EQ(nvbuf_surf->surfaceList->planeParams.pitch[0], 0x600u);
+ CHECK_EQ(nvbuf_surf->surfaceList->planeParams.pitch[1], 0x600u);
+ std::array<halide_dimension_t, 2> y_dimensions{{
+ {
+ /*.min =*/0,
+ /*.extent =*/
+ static_cast<int32_t>(nvbuf_surf->surfaceList->planeParams.width[0]),
+ /*.stride =*/1,
+ /*.flags =*/0,
+ },
+ {
+ /*.min = */ 0,
+ /*.extent =*/
+ static_cast<int32_t>(nvbuf_surf->surfaceList->planeParams.height[0]),
+ /*.stride =*/
+ static_cast<int32_t>(nvbuf_surf->surfaceList->planeParams.pitch[0]),
+ /*.flags =*/0,
+ },
+ }};
+
+ Halide::Runtime::Buffer<uint8_t, 2> y(
+ reinterpret_cast<uint8_t *>(nvbuf_surf->surfaceList->mappedAddr.addr[0]),
+ y_dimensions.size(), y_dimensions.data());
+
+ std::array<halide_dimension_t, 3> cbcr_dimensions{
+ {{
+ /*.min =*/0,
+ /*.extent =*/
+ static_cast<int32_t>(nvbuf_surf->surfaceList->planeParams.width[1]),
+ /*.stride =*/2,
+ /*.flags =*/0,
+ },
+ {
+ /*.min =*/0,
+ /*.extent =*/
+ static_cast<int32_t>(nvbuf_surf->surfaceList->planeParams.height[1]),
+ /*.stride =*/
+ static_cast<int32_t>(nvbuf_surf->surfaceList->planeParams.pitch[1]),
+ /*.flags =*/0,
+ },
+ {
+ /*.min =*/0,
+ /*.extent =*/2,
+ /*.stride =*/1,
+ /*.flags =*/0,
+ }}};
+
+ Halide::Runtime::Buffer<uint8_t, 3> cbcr(
+ reinterpret_cast<uint8_t *>(nvbuf_surf->surfaceList->mappedAddr.addr[1]),
+ cbcr_dimensions.size(), cbcr_dimensions.data());
+
+ std::array<halide_dimension_t, 3> output_dimensions{
+ {{
+ /*.min =*/0,
+ /*.extent =*/
+ static_cast<int32_t>(nvbuf_surf->surfaceList->planeParams.width[0]),
+ /*.stride =*/2,
+ /*.flags =*/0,
+ },
+ {
+ /*.min =*/0,
+ /*.extent =*/
+ static_cast<int32_t>(nvbuf_surf->surfaceList->planeParams.height[0]),
+ /*.stride =*/
+ static_cast<int32_t>(nvbuf_surf->surfaceList->planeParams.width[0] *
+ 2),
+ /*.flags =*/0,
+ },
+ {
+ /*.min =*/0,
+ /*.extent =*/2,
+ /*.stride =*/1,
+ /*.flags =*/0,
+ }}};
+
+ Halide::Runtime::Buffer<uint8_t, 3> output(
+ data_pointer, output_dimensions.size(), output_dimensions.data());
+ ycbcr422(y, cbcr, output);
+}
+
+// Helper class to tie a NvBufSurface to an Argus::Buffer.
+class DmaBuffer {
+ public:
+ // Creates a DmaBuffer. This is a static method so we can make sure it ends
+ // up as a unique_ptr so the pointer value doesn't change and break all the
+ // links.
+ static std::unique_ptr<DmaBuffer> Create(
+ const Argus::Size2D<uint32_t> &size, NvBufSurfaceColorFormat color_format,
+ NvBufSurfaceLayout layout = NVBUF_LAYOUT_PITCH) {
+ std::unique_ptr<DmaBuffer> buffer(new DmaBuffer());
+
+ NvBufSurfaceAllocateParams params;
+
+ params.memtag = NvBufSurfaceTag_CAMERA;
+ params.params.width = size.width();
+ params.params.height = size.height();
+ params.params.colorFormat = color_format;
+ params.params.layout = layout;
+ params.params.isContiguous = true;
+ params.disablePitchPadding = true;
+ params.params.memType = NVBUF_MEM_SURFACE_ARRAY;
+
+ NvBufSurface *nvbuf_surf = 0;
+ CHECK_EQ(NvBufSurfaceAllocate(&nvbuf_surf, 1, ¶ms), 0);
+ buffer->fd_ = nvbuf_surf->surfaceList[0].bufferDesc;
+
+ return buffer;
+ }
+
+ // Extracts the DmaBuffer from the Argus::Buffer.
+ static DmaBuffer *FromArgusBuffer(Argus::Buffer *buffer) {
+ Argus::IBuffer *i_buffer = Argus::interface_cast<Argus::IBuffer>(buffer);
+ const DmaBuffer *dmabuf =
+ static_cast<const DmaBuffer *>(i_buffer->getClientData());
+
+ return const_cast<DmaBuffer *>(dmabuf);
+ }
+
+ // Returns the DMA buffer handle.
+ int fd() const { return fd_; }
+
+ // Sets and gets the Argus::Buffer pointer.
+ void set_argus_buffer(Argus::Buffer *buffer) { buffer_ = buffer; }
+ Argus::Buffer *get_argus_buffer() const { return buffer_; }
+
+ virtual ~DmaBuffer() {
+ if (fd_ >= 0) {
+ NvBufSurface *nvbuf_surf = 0;
+ NvBufSurfaceFromFd(fd_, (void **)(&nvbuf_surf));
+ if (nvbuf_surf != NULL) {
+ NvBufSurfaceDestroy(nvbuf_surf);
+ }
+ }
+ }
+
+ private:
+ // Private to force people to use Create() above.
+ DmaBuffer() {}
+
+ int fd_ = -1;
+ Argus::Buffer *buffer_ = nullptr;
+};
+
+int Main() {
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig(FLAGS_config);
+
+ aos::ShmEventLoop event_loop(&config.message());
+
+ event_loop.SetRuntimeRealtimePriority(55);
+
+ aos::Sender<frc971::vision::CameraImage> sender =
+ event_loop.MakeSender<frc971::vision::CameraImage>(FLAGS_channel);
+
+ LOG(INFO) << "Started";
+ // Initialize the Argus camera provider.
+ Argus::UniqueObj<Argus::CameraProvider> camera_provider;
+ camera_provider =
+ Argus::UniqueObj<Argus::CameraProvider>(Argus::CameraProvider::create());
+
+ // Get the ICameraProvider interface from the global CameraProvider
+ Argus::ICameraProvider *i_camera_provider =
+ Argus::interface_cast<Argus::ICameraProvider>(camera_provider);
+ if (!i_camera_provider) {
+ ORIGINATE_ERROR("Failed to get ICameraProvider interface");
+ }
+
+ // Get the camera devices.
+ std::vector<Argus::CameraDevice *> camera_devices;
+ i_camera_provider->getCameraDevices(&camera_devices);
+ if (camera_devices.size() == 0) {
+ ORIGINATE_ERROR("there are %d cameras", (unsigned)camera_devices.size());
+ }
+
+ LOG(INFO) << "Found " << camera_devices.size() << " cameras";
+ for (Argus::CameraDevice *camera : camera_devices) {
+ Argus::ICameraProperties *iCameraProperties =
+ Argus::interface_cast<Argus::ICameraProperties>(camera);
+ LOG(INFO) << "Camera " << iCameraProperties->getModelName();
+ }
+
+ std::vector<Argus::SensorMode *> sensor_modes;
+ Argus::ICameraProperties *iCameraProperties =
+ Argus::interface_cast<Argus::ICameraProperties>(
+ camera_devices[FLAGS_camera]);
+ if (!iCameraProperties)
+ ORIGINATE_ERROR("Failed to get ICameraProperties Interface");
+ // Get available Sensor Modes
+ iCameraProperties->getAllSensorModes(&sensor_modes);
+ LOG(INFO) << "Found " << sensor_modes.size() << " modes";
+
+ for (Argus::SensorMode *mode : sensor_modes) {
+ Argus::ISensorMode *imode = Argus::interface_cast<Argus::ISensorMode>(mode);
+ LOG(INFO) << imode->getResolution().width() << " x "
+ << imode->getResolution().height();
+ LOG(INFO) << "type " << imode->getSensorModeType().getName();
+ LOG(INFO) << "exposure min " << imode->getExposureTimeRange().min();
+ LOG(INFO) << "exposure max " << imode->getExposureTimeRange().max();
+ }
+ if (sensor_modes.size() <= 0) {
+ ORIGINATE_ERROR("Preview Sensor Mode %d not available", 0);
+ }
+
+ Argus::ISensorMode *i_sensor_mode =
+ Argus::interface_cast<Argus::ISensorMode>(sensor_modes[FLAGS_mode]);
+ if (!i_sensor_mode) {
+ ORIGINATE_ERROR("Failed to get SensorMode interface");
+ }
+
+ {
+ auto range = i_sensor_mode->getFrameDurationRange();
+ LOG(INFO) << "Min: " << range.min() << ", " << range.max();
+ LOG(INFO) << "type " << i_sensor_mode->getSensorModeType().getName();
+ }
+
+ // Create the capture session using the first device and get the core
+ // interface.
+ Argus::UniqueObj<Argus::CaptureSession> capture_session;
+ capture_session.reset(
+ i_camera_provider->createCaptureSession(camera_devices[FLAGS_camera]));
+ Argus::ICaptureSession *i_capture_session =
+ Argus::interface_cast<Argus::ICaptureSession>(capture_session);
+ if (!i_capture_session) {
+ ORIGINATE_ERROR("Failed to create CaptureSession");
+ }
+
+ EGLDisplay egl_display = eglGetDisplay(EGL_DEFAULT_DISPLAY);
+ CHECK_NE(egl_display, EGL_NO_DISPLAY) << ": Failed to open display";
+
+ // Create the OutputStream.
+ Argus::UniqueObj<Argus::OutputStreamSettings> stream_settings(
+ i_capture_session->createOutputStreamSettings(Argus::STREAM_TYPE_BUFFER));
+
+ Argus::IBufferOutputStreamSettings *i_buffer_output_stream_settings =
+ Argus::interface_cast<Argus::IBufferOutputStreamSettings>(
+ stream_settings);
+ CHECK(i_buffer_output_stream_settings != nullptr);
+ i_buffer_output_stream_settings->setBufferType(Argus::BUFFER_TYPE_EGL_IMAGE);
+ i_buffer_output_stream_settings->setMetadataEnable(true);
+ LOG(INFO) << "Type: "
+ << i_buffer_output_stream_settings->getBufferType().getName();
+
+ Argus::UniqueObj<Argus::OutputStream> output_stream(
+ i_capture_session->createOutputStream(stream_settings.get()));
+ LOG(INFO) << "Got sream";
+
+ Argus::IBufferOutputStream *i_buffer_output_stream =
+ Argus::interface_cast<Argus::IBufferOutputStream>(output_stream);
+ CHECK(i_buffer_output_stream != nullptr);
+
+ // Build the DmaBuffers
+ std::array<std::unique_ptr<DmaBuffer>, 10> native_buffers;
+ for (size_t i = 0; i < native_buffers.size(); ++i) {
+ native_buffers[i] = DmaBuffer::Create(
+ i_sensor_mode->getResolution(),
+ static_cast<NvBufSurfaceColorFormat>(FLAGS_colorformat),
+ NVBUF_LAYOUT_PITCH);
+ }
+
+ std::array<NvBufSurface *, 10> surf;
+
+ // Create EGLImages from the native buffers
+ std::array<EGLImageKHR, 10> egl_images;
+ for (size_t i = 0; i < egl_images.size(); i++) {
+ int ret = 0;
+
+ ret = NvBufSurfaceFromFd(native_buffers[i]->fd(), (void **)(&surf[i]));
+ CHECK(ret == 0) << ": NvBufSurfaceFromFd failed";
+
+ ret = NvBufSurfaceMapEglImage(surf[i], 0);
+ CHECK(ret == 0) << ": NvBufSurfaceMapEglImage failed";
+
+ egl_images[i] = surf[i]->surfaceList[0].mappedAddr.eglImage;
+ CHECK(egl_images[i] != EGL_NO_IMAGE_KHR) << ": Failed to create EGLImage";
+ }
+
+ // Create the BufferSettings object to configure Buffer creation.
+ Argus::UniqueObj<Argus::BufferSettings> buffer_settings(
+ i_buffer_output_stream->createBufferSettings());
+ Argus::IEGLImageBufferSettings *i_buffer_settings =
+ Argus::interface_cast<Argus::IEGLImageBufferSettings>(buffer_settings);
+ if (!i_buffer_settings) ORIGINATE_ERROR("Failed to create BufferSettings");
+
+ // Create the Buffers for each EGLImage (and release to the stream for initial
+ // capture use)
+ std::array<Argus::UniqueObj<Argus::Buffer>, 10> buffers;
+ for (size_t i = 0; i < buffers.size(); i++) {
+ i_buffer_settings->setEGLImage(egl_images[i]);
+ i_buffer_settings->setEGLDisplay(egl_display);
+ buffers[i].reset(
+ i_buffer_output_stream->createBuffer(buffer_settings.get()));
+ Argus::IBuffer *i_buffer =
+ Argus::interface_cast<Argus::IBuffer>(buffers[i]);
+
+ // Ties Argus::Buffer and DmaBuffer together.
+ i_buffer->setClientData(native_buffers[i].get());
+ native_buffers[i]->set_argus_buffer(buffers[i].get());
+
+ CHECK(Argus::interface_cast<Argus::IEGLImageBuffer>(buffers[i]) != nullptr)
+ << ": Failed to create Buffer";
+
+ if (i_buffer_output_stream->releaseBuffer(buffers[i].get()) !=
+ Argus::STATUS_OK)
+ ORIGINATE_ERROR("Failed to release Buffer for capture use");
+ }
+
+ Argus::UniqueObj<Argus::Request> request(i_capture_session->createRequest());
+ Argus::IRequest *i_request = Argus::interface_cast<Argus::IRequest>(request);
+ CHECK(i_request);
+
+ Argus::IAutoControlSettings *i_auto_control_settings =
+ Argus::interface_cast<Argus::IAutoControlSettings>(
+ i_request->getAutoControlSettings());
+ CHECK(i_auto_control_settings != nullptr);
+ i_auto_control_settings->setAwbMode(Argus::AWB_MODE_OFF);
+
+ i_auto_control_settings->setAeLock(false);
+ Argus::Range<float> isp_digital_gain_range;
+ isp_digital_gain_range.min() = 1;
+ isp_digital_gain_range.max() = 1;
+ i_auto_control_settings->setIspDigitalGainRange(isp_digital_gain_range);
+
+ Argus::IEdgeEnhanceSettings *i_ee_settings =
+ Argus::interface_cast<Argus::IEdgeEnhanceSettings>(request);
+ CHECK(i_ee_settings != nullptr);
+
+ i_ee_settings->setEdgeEnhanceStrength(0);
+
+ i_request->enableOutputStream(output_stream.get());
+
+ Argus::ISourceSettings *i_source_settings =
+ Argus::interface_cast<Argus::ISourceSettings>(
+ i_request->getSourceSettings());
+ CHECK(i_source_settings != nullptr);
+
+ i_source_settings->setFrameDurationRange(
+ i_sensor_mode->getFrameDurationRange().min());
+ i_source_settings->setSensorMode(sensor_modes[FLAGS_mode]);
+
+ Argus::Range<float> sensor_mode_analog_gain_range;
+ sensor_mode_analog_gain_range.min() = FLAGS_gain;
+ sensor_mode_analog_gain_range.max() = FLAGS_gain;
+ i_source_settings->setGainRange(sensor_mode_analog_gain_range);
+
+ Argus::Range<uint64_t> limit_exposure_time_range;
+ limit_exposure_time_range.min() = FLAGS_exposure;
+ limit_exposure_time_range.max() = FLAGS_exposure;
+ i_source_settings->setExposureTimeRange(limit_exposure_time_range);
+
+ if (i_capture_session->repeat(request.get()) != Argus::STATUS_OK) {
+ LOG(ERROR) << "Failed to submit repeat";
+ }
+
+ LOG(INFO) << "Session submitted";
+
+ // Run.
+ //
+ // TODO(austin): Use the event loop a bit better... That'll let us set
+ // priority + get stats. Timer which always repeats "now" ?
+ aos::monotonic_clock::time_point last_time = aos::monotonic_clock::epoch();
+ while (true) {
+ VLOG(1) << "Going for frame";
+ Argus::Status status;
+
+ Argus::Buffer *buffer = i_buffer_output_stream->acquireBuffer(
+ std::chrono::nanoseconds(std::chrono::seconds(5)).count(), &status);
+
+ if (status == Argus::STATUS_END_OF_STREAM) {
+ break;
+ }
+
+ const aos::monotonic_clock::time_point now = aos::monotonic_clock::now();
+
+ DmaBuffer *dmabuf = DmaBuffer::FromArgusBuffer(buffer);
+ int dmabuf_fd = dmabuf->fd();
+
+ Argus::IBuffer *ibuffer = Argus::interface_cast<Argus::IBuffer>(buffer);
+ CHECK(ibuffer != nullptr);
+
+ const Argus::CaptureMetadata *metadata = ibuffer->getMetadata();
+ const Argus::ICaptureMetadata *imetadata =
+ Argus::interface_cast<const Argus::ICaptureMetadata>(metadata);
+
+ NvBufSurface *nvbuf_surf = 0;
+ CHECK_EQ(NvBufSurfaceFromFd(dmabuf_fd, (void **)(&nvbuf_surf)), 0);
+
+ CHECK_EQ(NvBufSurfaceMap(nvbuf_surf, -1, -1, NVBUF_MAP_READ), 0);
+ VLOG(1) << "Mapped";
+ NvBufSurfaceSyncForCpu(nvbuf_surf, -1, -1);
+
+ VLOG(1) << "Planes " << nvbuf_surf->surfaceList->planeParams.num_planes
+ << " colorFormat " << nvbuf_surf->surfaceList->colorFormat;
+ for (size_t i = 0; i < nvbuf_surf->surfaceList->planeParams.num_planes;
+ ++i) {
+ VLOG(1) << "Address "
+ << static_cast<void *>(
+ nvbuf_surf->surfaceList->mappedAddr.addr[i])
+ << ", pitch " << nvbuf_surf->surfaceList->planeParams.pitch[i]
+ << " height " << nvbuf_surf->surfaceList->planeParams.height[i]
+ << " width " << nvbuf_surf->surfaceList->planeParams.width[i]
+ << " bytes per pixel "
+ << nvbuf_surf->surfaceList->planeParams.bytesPerPix[i];
+ }
+ CHECK_EQ(nvbuf_surf->surfaceList->planeParams.width[0],
+ static_cast<size_t>(FLAGS_width));
+ CHECK_EQ(nvbuf_surf->surfaceList->planeParams.height[0],
+ static_cast<size_t>(FLAGS_height));
+
+ aos::Sender<frc971::vision::CameraImage>::Builder builder =
+ sender.MakeBuilder();
+
+ uint8_t *data_pointer = nullptr;
+ builder.fbb()->StartIndeterminateVector(FLAGS_width * FLAGS_height * 2, 1,
+ 64, &data_pointer);
+
+ YCbCr422(nvbuf_surf, data_pointer);
+ flatbuffers::Offset<flatbuffers::Vector<uint8_t>> data_offset =
+ builder.fbb()->EndIndeterminateVector(FLAGS_width * FLAGS_height * 2,
+ 1);
+
+ auto image_builder = builder.MakeBuilder<frc971::vision::CameraImage>();
+ image_builder.add_data(data_offset);
+ image_builder.add_rows(FLAGS_height);
+ image_builder.add_cols(FLAGS_width);
+ image_builder.add_monotonic_timestamp_ns(imetadata->getFrameReadoutTime());
+ builder.CheckOk(builder.Send(image_builder.Finish()));
+
+ const aos::monotonic_clock::time_point after_send =
+ aos::monotonic_clock::now();
+
+ CHECK_EQ(NvBufSurfaceUnMap(nvbuf_surf, -1, -1), 0);
+
+ CHECK(imetadata);
+ VLOG(1) << "Got " << imetadata->getCaptureId() << " delay "
+ << chrono::duration<double>(
+ chrono::nanoseconds((now.time_since_epoch().count() -
+ (imetadata->getSensorTimestamp() +
+ imetadata->getFrameReadoutTime()))))
+ .count()
+ << " mmap " << chrono::duration<double>(after_send - now).count()
+ << "sec dt " << chrono::duration<double>(now - last_time).count()
+ << "sec " << dmabuf << " exposure "
+ << imetadata->getSensorExposureTime();
+ i_buffer_output_stream->releaseBuffer(buffer);
+
+ last_time = now;
+ }
+
+ i_capture_session->stopRepeat();
+ i_buffer_output_stream->endOfStream();
+ i_capture_session->waitForIdle();
+
+ output_stream.reset();
+
+ for (uint32_t i = 0; i < surf.size(); i++) {
+ NvBufSurfaceUnMapEglImage(surf[i], 0);
+ }
+
+ eglTerminate(egl_display);
+ return 0;
+}
+
+}; // namespace frc971
+
+int main(int argc, char **argv) {
+ aos::InitGoogle(&argc, &argv);
+ return frc971::Main();
+}
+
+// I tried every different format option. Here's what worked and didn't work.
+//
+// NVBUF_COLOR_FORMAT_RGB,
+
+// NVBUF_COLOR_FORMAT_YUYV, // Failed
+// NVBUF_COLOR_FORMAT_NV24, // Works
+// NVBUF_COLOR_FORMAT_UYVY, // Failed
+// NVBUF_COLOR_FORMAT_YUV420, // Failed with error.
+
+// NVBUF_COLOR_FORMAT_GRAY8, // unsupported
+// NVBUF_COLOR_FORMAT_YUV420, // unsupported
+// NVBUF_COLOR_FORMAT_YVU420, // unsupported
+
+// NVBUF_COLOR_FORMAT_YUV420_ER, // unsupported
+// NVBUF_COLOR_FORMAT_YVU420_ER, // unsupported
+//
+///** Specifies BT.601 colorspace - Y/CbCr 4:2:0 multi-planar. */
+// NVBUF_COLOR_FORMAT_NV12, // Works! pitch 2048 height 1080 width
+// 1920 colorFormat 6 planes 2 bytes per pixel 1 delay 0.00203304
+// mmap 0.000340288sec dt 0.0166379sec
+//
+///** Specifies BT.601 colorspace - Y/CbCr ER 4:2:0 multi-planar. */
+// NVBUF_COLOR_FORMAT_NV12_ER, // Works! pitch 2048 height 1080
+// width 1920 colorFormat 7 planes 2 bytes per pixel 1
+///** Specifies BT.601 colorspace - Y/CbCr 4:2:0 multi-planar. */
+// NVBUF_COLOR_FORMAT_NV21, // Works! pitch 2048 height 1080 width
+// 1920 colorFormat 8 planes 2 bytes per pixel 1
+///** Specifies BT.601 colorspace - Y/CbCr ER 4:2:0 multi-planar. */
+// NVBUF_COLOR_FORMAT_NV21_ER, // Works! pitch 2048 height 1080
+// width 1920 colorFormat 9 planes 2 bytes per pixel 1
+//
+//
+// NVBUF_COLOR_FORMAT_UYVY, // works with an error?!?
+// NVBUF_COLOR_FORMAT_UYVY_ER, // unsupported 11
+// NVBUF_COLOR_FORMAT_VYUY, // unsupported 12
+// NVBUF_COLOR_FORMAT_VYUY_ER, // unsupported 13
+// NVBUF_COLOR_FORMAT_YUYV, // unsupported 14
+// NVBUF_COLOR_FORMAT_YUYV_ER, // unsupported 15
+// NVBUF_COLOR_FORMAT_YVYU, // unsupported 16
+// NVBUF_COLOR_FORMAT_YVYU_ER, // unsupported 17
+// NVBUF_COLOR_FORMAT_YUV444, // unsupported 18
+// NVBUF_COLOR_FORMAT_RGBA, // unsupported 19
+// NVBUF_COLOR_FORMAT_BGRA, // unsupported 20
+// NVBUF_COLOR_FORMAT_ARGB, // unsupported 21
+// NVBUF_COLOR_FORMAT_ABGR, // unsupported 22
+// NVBUF_COLOR_FORMAT_RGBx, // unsupported 23
+// NVBUF_COLOR_FORMAT_BGRx, // unsupported 24
+// NVBUF_COLOR_FORMAT_xRGB, // unsupported 25
+// NVBUF_COLOR_FORMAT_xBGR, // unsupported 26
+// NVBUF_COLOR_FORMAT_RGB, // unsupported 27
+// NVBUF_COLOR_FORMAT_BGR, // unsupported 28
+// NVBUF_COLOR_FORMAT_NV12_10LE, // unsupported 29
+// NVBUF_COLOR_FORMAT_NV12_12LE, // unsupported 30
+// NVBUF_COLOR_FORMAT_YUV420_709, // unsupported 31
+// NVBUF_COLOR_FORMAT_YUV420_709_ER, // unsupported 32
+// NVBUF_COLOR_FORMAT_NV12_709, // works pitch 2048 height 1080
+// width 1920 colorFormat 33 planes 2 bytes per pixel 1
+// NVBUF_COLOR_FORMAT_NV12_709_ER, // works pitch 2048 height 1080
+// width 1920 colorFormat 34 planes 2 bytes per pixel 1
+// NVBUF_COLOR_FORMAT_YUV420_2020, // unsupported 35
+// NVBUF_COLOR_FORMAT_NV12_2020, // unsupported 36
+// NVBUF_COLOR_FORMAT_NV12_10LE_ER, // unsupported 37
+// NVBUF_COLOR_FORMAT_NV12_10LE_709, // unsupported 38
+// NVBUF_COLOR_FORMAT_NV12_10LE_709_ER, // unsupported 39
+// NVBUF_COLOR_FORMAT_NV12_10LE_2020, // unsupported 40
+// NVBUF_COLOR_FORMAT_SIGNED_R16G16, // unsupported 41
+// NVBUF_COLOR_FORMAT_R8_G8_B8, // unsupported 42
+// NVBUF_COLOR_FORMAT_B8_G8_R8, // unsupported 43
+// NVBUF_COLOR_FORMAT_R32F_G32F_B32F, // unsupported 44
+// NVBUF_COLOR_FORMAT_B32F_G32F_R32F, // unsupported 45
+// NVBUF_COLOR_FORMAT_YUV422, // unsupported 46
+// NVBUF_COLOR_FORMAT_NV21_10LE, // unsupported 47
+// NVBUF_COLOR_FORMAT_NV21_12LE, // unsupported 48
+// NVBUF_COLOR_FORMAT_NV12_12LE_2020, // unsupported 49
+///** Specifies BT.601 colorspace - Y/CbCr 4:2:2 multi-planar. */
+// NVBUF_COLOR_FORMAT_NV16, // works pitch 2048 height 1080 width
+// 1920 colorFormat 50 planes 2 bytes per pixel 1
+// NVBUF_COLOR_FORMAT_NV16_10LE, // unsupported 51
+///** Specifies BT.601 colorspace - Y/CbCr 4:4:4 multi-planar. */
+// NVBUF_COLOR_FORMAT_NV24, // works pitch 2048 height 1080
+// width 1920 colorFormat 52 planes 2 bytes per pixel 1
+// NVBUF_COLOR_FORMAT_NV24_10LE, // unsupported 53
+//
+// NVBUF_COLOR_FORMAT_NV16_ER, // works pitch 2048 height 1080
+// width 1920 colorFormat 54 planes 2 bytes per pixel 1
+// NVBUF_COLOR_FORMAT_NV24_ER, // works pitch 2048 height 1080
+// width 1920 colorFormat 55 planes 2 bytes per pixel 1
+// NVBUF_COLOR_FORMAT_NV16_709, // unsupported 56
+// NVBUF_COLOR_FORMAT_NV24_709, // unsupported 57
+// NVBUF_COLOR_FORMAT_NV16_709_ER, // unsupported 58
+// NVBUF_COLOR_FORMAT_NV24_709_ER, // unsupported 59
+// NVBUF_COLOR_FORMAT_NV24_10LE_709, // unsupported 60
+// NVBUF_COLOR_FORMAT_NV24_10LE_709_ER, // unsupported 61
+// NVBUF_COLOR_FORMAT_NV24_10LE_2020, // unsupported 62
+// NVBUF_COLOR_FORMAT_NV24_12LE_2020, // unsupported 63
+// NVBUF_COLOR_FORMAT_RGBA_10_10_10_2_709, // unsupported 64
+// NVBUF_COLOR_FORMAT_RGBA_10_10_10_2_2020, // unsupported 65
+// NVBUF_COLOR_FORMAT_BGRA_10_10_10_2_709, // unsupported 66
+// NVBUF_COLOR_FORMAT_BGRA_10_10_10_2_2020, // unsupported 67
+// NVBUF_COLOR_FORMAT_A32, // unsupported 68
+// NVBUF_COLOR_FORMAT_UYVP, // unsupported 69
+// NVBUF_COLOR_FORMAT_UYVP_ER // unsupported 70
+
+// NVBUF_COLOR_FORMAT_ABGR,
+// NVBUF_COLOR_FORMAT_ARGB,
diff --git a/frc971/orin/build_rootfs.py b/frc971/orin/build_rootfs.py
index ceed056..c378226 100755
--- a/frc971/orin/build_rootfs.py
+++ b/frc971/orin/build_rootfs.py
@@ -765,7 +765,7 @@
resolved_deps.sort()
rule_name = obj[1:].replace('/', '_')
rule_deps = ''.join([
- ' ":%s",\n'.format(d[1:].replace('/', '_'))
+ ' ":{}",\n'.format(d[1:].replace('/', '_'))
for d in resolved_deps if d not in skip_set
])
rules.append(
@@ -835,10 +835,6 @@
],
exclude = [
"usr/share/**",
- "usr/local/cuda-11.8/include/thrust/**",
- "usr/local/cuda-11.8/include/nv/**",
- "usr/local/cuda-11.8/include/cuda/**",
- "usr/local/cuda-11.8/include/cub/**",
],
)""",
"RULES": '\n\n'.join(rules),
@@ -847,6 +843,8 @@
with open("../../compilers/orin_debian_rootfs.BUILD", "w") as file:
file.write(template.render(substitutions))
+ subprocess.run(['buildifier', "../../compilers/orin_debian_rootfs.BUILD"])
+
def do_package(partition):
tarball = datetime.date.today().strftime(
@@ -875,6 +873,10 @@
"--exclude=./usr/local/cuda-11.8/include/nv",
"--exclude=./usr/local/cuda-11.8/include/cuda",
"--exclude=./usr/local/cuda-11.8/include/cub",
+ "--exclude=./usr/include/cub",
+ "--exclude=./usr/include/nv",
+ "--exclude=./usr/include/thrust",
+ "--exclude=./usr/include/cuda",
"--exclude=./usr/share",
"-cf",
tarball,
@@ -988,7 +990,8 @@
"gstreamer1.0-nice", "usbutils", "locales", "trace-cmd", "clinfo",
"jq", "strace", "sysstat", "lm-sensors", "can-utils", "xfsprogs",
"gstreamer1.0-tools", "bridge-utils", "net-tools", "apt-file",
- "parted", "xxd", "libv4l-dev", "file", "pkexec", "libxkbfile1"
+ "parted", "xxd", "libv4l-dev", "file", "pkexec", "libxkbfile1",
+ "gdb"
])
target(["apt-get", "clean"])
@@ -1043,6 +1046,7 @@
'cuda-nvcc-headers-11-8',
'nsight-systems-cli',
'nsight-systems-cli-qdstrmimporter',
+ 'tegra-tools-jetson-clocks',
]
yocto_packages = list_yocto_packages()
packages = list_packages()
@@ -1098,6 +1102,8 @@
copyfile("pi:pi", "600", "home/pi/.ssh/authorized_keys")
target_mkdir("root:root", "700", "root/bin")
copyfile("root:root", "644", "etc/systemd/system/grow-rootfs.service")
+ copyfile("root:root", "644",
+ "etc/systemd/system/jetson-clocks.service")
copyfile("root:root", "500", "root/bin/change_hostname.sh")
copyfile("root:root", "700", "root/trace.sh")
copyfile("root:root", "440", "etc/sudoers")
@@ -1108,12 +1114,16 @@
target_mkdir("root:root", "755", "etc/systemd/network")
copyfile("root:root", "644", "etc/systemd/network/eth0.network")
- copyfile("root:root", "644", "etc/systemd/network/80-can.network")
+ copyfile("root:root", "644", "etc/systemd/network/80-cana.network")
+ copyfile("root:root", "644", "etc/systemd/network/80-canb.network")
+ copyfile("root:root", "644", "etc/systemd/network/80-canc.network")
copyfile("root:root", "644", "etc/udev/rules.d/nvidia.rules")
+ copyfile("root:root", "644", "etc/udev/rules.d/can.rules")
target(["/root/bin/change_hostname.sh", "pi-971-1"])
target(["systemctl", "enable", "systemd-networkd"])
target(["systemctl", "enable", "grow-rootfs"])
+ target(["systemctl", "enable", "jetson-clocks"])
target(["apt-file", "update"])
diff --git a/frc971/orin/contents/etc/systemd/network/80-can.network b/frc971/orin/contents/etc/systemd/network/80-cana.network
similarity index 64%
copy from frc971/orin/contents/etc/systemd/network/80-can.network
copy to frc971/orin/contents/etc/systemd/network/80-cana.network
index e75db33..0e4ad11 100644
--- a/frc971/orin/contents/etc/systemd/network/80-can.network
+++ b/frc971/orin/contents/etc/systemd/network/80-cana.network
@@ -1,9 +1,7 @@
[Match]
-Name=can0
+Name=cana
[CAN]
BitRate=1M
-DataBitRate=8M
RestartSec=1000ms
BusErrorReporting=yes
-FDMode=yes
diff --git a/frc971/orin/contents/etc/systemd/network/80-can.network b/frc971/orin/contents/etc/systemd/network/80-canb.network
similarity index 90%
rename from frc971/orin/contents/etc/systemd/network/80-can.network
rename to frc971/orin/contents/etc/systemd/network/80-canb.network
index e75db33..1bef0ee 100644
--- a/frc971/orin/contents/etc/systemd/network/80-can.network
+++ b/frc971/orin/contents/etc/systemd/network/80-canb.network
@@ -1,5 +1,5 @@
[Match]
-Name=can0
+Name=canb
[CAN]
BitRate=1M
diff --git a/frc971/orin/contents/etc/systemd/network/80-can.network b/frc971/orin/contents/etc/systemd/network/80-canc.network
similarity index 90%
copy from frc971/orin/contents/etc/systemd/network/80-can.network
copy to frc971/orin/contents/etc/systemd/network/80-canc.network
index e75db33..cdcbe81 100644
--- a/frc971/orin/contents/etc/systemd/network/80-can.network
+++ b/frc971/orin/contents/etc/systemd/network/80-canc.network
@@ -1,5 +1,5 @@
[Match]
-Name=can0
+Name=canc
[CAN]
BitRate=1M
diff --git a/frc971/orin/contents/etc/systemd/system/jetson-clocks.service b/frc971/orin/contents/etc/systemd/system/jetson-clocks.service
new file mode 100644
index 0000000..9e67379
--- /dev/null
+++ b/frc971/orin/contents/etc/systemd/system/jetson-clocks.service
@@ -0,0 +1,16 @@
+[Install]
+WantedBy=multi-user.target
+
+[Unit]
+Description=Service to pin the device to max clock rate
+
+[Service]
+Type=oneshot
+
+# Turn the GPU on
+ExecStart=bash -c 'echo 1 > /dev/nvgpu/igpu0/power'
+# Set all the clocks to max frequency
+ExecStart=/usr/bin/jetson_clocks
+
+TimeoutSec=0
+RemainAfterExit=yes
diff --git a/frc971/orin/contents/etc/udev/rules.d/can.rules b/frc971/orin/contents/etc/udev/rules.d/can.rules
new file mode 100644
index 0000000..83aa68e
--- /dev/null
+++ b/frc971/orin/contents/etc/udev/rules.d/can.rules
@@ -0,0 +1,3 @@
+SUBSYSTEM=="net", ACTION=="add", ATTR{dev_id}=="0x0", ENV{ID_PATH_TAG}=="platform-c310000_mttcan", NAME="cana"
+SUBSYSTEM=="net", ACTION=="add", ATTR{dev_id}=="0x0", ENV{ID_PATH_TAG}=="platform-14100000_pcie-pci-0001_01_00_0", NAME="canb"
+SUBSYSTEM=="net", ACTION=="add", ATTR{dev_id}=="0x1", ENV{ID_PATH_TAG}=="platform-14100000_pcie-pci-0001_01_00_0", NAME="canc"
diff --git a/frc971/orin/crcv_generator.cc b/frc971/orin/crcv_generator.cc
index 99318e6..5c72385 100644
--- a/frc971/orin/crcv_generator.cc
+++ b/frc971/orin/crcv_generator.cc
@@ -20,17 +20,17 @@
namespace {
template <typename T>
-void SetRowMajor(T *buffer_parameter, int cols, int rows) {
- buffer_parameter->dim(0).set_stride(3);
+void SetRowMajor(T *buffer_parameter, int cols, int rows, int channels) {
+ buffer_parameter->dim(0).set_stride(channels);
buffer_parameter->dim(0).set_extent(cols);
buffer_parameter->dim(0).set_min(0);
- buffer_parameter->dim(1).set_stride(cols * 3);
+ buffer_parameter->dim(1).set_stride(cols * channels);
buffer_parameter->dim(1).set_extent(rows);
buffer_parameter->dim(1).set_min(0);
buffer_parameter->dim(2).set_stride(1);
- buffer_parameter->dim(2).set_extent(3);
+ buffer_parameter->dim(2).set_extent(channels);
buffer_parameter->dim(2).set_min(0);
}
} // namespace
@@ -86,7 +86,59 @@
output.vectorize(col, 8);
output.unroll(col, 4);
- SetRowMajor(&output, cols, rows);
+ SetRowMajor(&output, cols, rows, 3);
+ }
+};
+
+class YCbCr422 : public Halide::Generator<YCbCr422> {
+ public:
+ GeneratorParam<int> cols{"cols", 0};
+ GeneratorParam<int> rows{"rows", 0};
+ GeneratorParam<int> ystride{"ystride", 0};
+ GeneratorParam<int> cbcrstride{"cbcrstride", 0};
+
+ Input<Buffer<uint8_t, 2>> input_y{"y"};
+ Input<Buffer<uint8_t, 3>> input_cbcr{"cbcr"};
+ Output<Buffer<uint8_t, 3>> output{"output"};
+
+ Var col{"col"}, row{"row"}, channel{"channel"};
+
+ // Everything is indexed as col, row, channel.
+ void generate() {
+ CHECK(cols > 0, "Must specify a cols");
+ CHECK((cols % 2) == 0, "Must specify a cols with an even number of cols");
+ CHECK(rows > 0, "Must specify a rows");
+
+ input_y.dim(0).set_stride(1);
+ input_y.dim(0).set_extent(cols);
+ input_y.dim(0).set_min(0);
+
+ input_y.dim(1).set_stride(ystride);
+ input_y.dim(1).set_extent(rows);
+ input_y.dim(1).set_min(0);
+
+ input_cbcr.dim(0).set_stride(2);
+ input_cbcr.dim(0).set_extent(cols / 2);
+ input_cbcr.dim(0).set_min(0);
+
+ input_cbcr.dim(1).set_stride(cbcrstride);
+ input_cbcr.dim(1).set_extent(rows);
+ input_cbcr.dim(1).set_min(0);
+
+ input_cbcr.dim(2).set_stride(1);
+ input_cbcr.dim(2).set_extent(2);
+ input_cbcr.dim(2).set_min(0);
+
+ output(col, row, channel) = Halide::select(
+ channel == 0, input_y(col, row), input_cbcr(col / 2, row, col % 2));
+
+ output.reorder(channel, col, row);
+ output.unroll(channel);
+
+ output.vectorize(col, 8);
+ output.unroll(col, 4);
+
+ SetRowMajor(&output, cols, rows, 2);
}
};
@@ -94,3 +146,4 @@
} // namespace frc971
HALIDE_REGISTER_GENERATOR(frc971::orin::YCbCr, ycbcr)
+HALIDE_REGISTER_GENERATOR(frc971::orin::YCbCr422, ycbcr422)
diff --git a/frc971/orin/cuda.cc b/frc971/orin/cuda.cc
new file mode 100644
index 0000000..1173f73
--- /dev/null
+++ b/frc971/orin/cuda.cc
@@ -0,0 +1,23 @@
+#include "frc971/orin/cuda.h"
+
+#include "gflags/gflags.h"
+#include "glog/logging.h"
+
+DEFINE_bool(
+ sync, false,
+ "If true, force synchronization after each step to isolate errors better.");
+
+namespace frc971 {
+namespace apriltag {
+
+void CheckAndSynchronize() {
+ CHECK_CUDA(cudaDeviceSynchronize());
+ CHECK_CUDA(cudaGetLastError());
+}
+
+void MaybeCheckAndSynchronize() {
+ if (FLAGS_sync) CheckAndSynchronize();
+}
+
+} // namespace apriltag
+} // namespace frc971
diff --git a/frc971/orin/cuda.h b/frc971/orin/cuda.h
new file mode 100644
index 0000000..e386aa6
--- /dev/null
+++ b/frc971/orin/cuda.h
@@ -0,0 +1,207 @@
+#ifndef FRC971_ORIN_CUDA_H_
+#define FRC971_ORIN_CUDA_H_
+
+#include <chrono>
+#include <span>
+
+#include "glog/logging.h"
+
+#include "cuda_runtime.h"
+#include "device_launch_parameters.h"
+
+// CHECKs that a cuda method returned success.
+// TODO(austin): This will not handle if and else statements quite right, fix if
+// we care.
+#define CHECK_CUDA(condition) \
+ if (auto c = condition) \
+ LOG(FATAL) << "Check failed: " #condition " (" << cudaGetErrorString(c) \
+ << ") "
+
+namespace frc971 {
+namespace apriltag {
+
+// Class to manage the lifetime of a Cuda stream. This is used to provide
+// relative ordering between kernels on the same stream.
+class CudaStream {
+ public:
+ CudaStream() { CHECK_CUDA(cudaStreamCreate(&stream_)); }
+
+ CudaStream(const CudaStream &) = delete;
+ CudaStream &operator=(const CudaStream &) = delete;
+
+ virtual ~CudaStream() { CHECK_CUDA(cudaStreamDestroy(stream_)); }
+
+ // Returns the stream.
+ cudaStream_t get() { return stream_; }
+
+ private:
+ cudaStream_t stream_;
+};
+
+// Class to manage the lifetime of a Cuda Event. Cuda events are used for
+// timing events on a stream.
+class CudaEvent {
+ public:
+ CudaEvent() { CHECK_CUDA(cudaEventCreate(&event_)); }
+
+ CudaEvent(const CudaEvent &) = delete;
+ CudaEvent &operator=(const CudaEvent &) = delete;
+
+ virtual ~CudaEvent() { CHECK_CUDA(cudaEventDestroy(event_)); }
+
+ // Queues up an event to be timestamped on the stream when it is executed.
+ void Record(CudaStream *stream) {
+ CHECK_CUDA(cudaEventRecord(event_, stream->get()));
+ }
+
+ // Returns the time elapsed between start and this event if it has been
+ // triggered.
+ std::chrono::nanoseconds ElapsedTime(const CudaEvent &start) {
+ float ms;
+ CHECK_CUDA(cudaEventElapsedTime(&ms, start.event_, event_));
+ return std::chrono::duration_cast<std::chrono::nanoseconds>(
+ std::chrono::duration<float, std::milli>(ms));
+ }
+
+ // Waits until the event has been triggered.
+ void Synchronize() { CHECK_CUDA(cudaEventSynchronize(event_)); }
+
+ private:
+ cudaEvent_t event_;
+};
+
+// Class to manage the lifetime of page locked host memory for fast copies back
+// to host memory.
+template <typename T>
+class HostMemory {
+ public:
+ // Allocates a block of memory for holding up to size objects of type T.
+ HostMemory(size_t size) {
+ T *memory;
+ CHECK_CUDA(cudaMallocHost((void **)(&memory), size * sizeof(T)));
+ span_ = std::span<T>(memory, size);
+ }
+ HostMemory(const HostMemory &) = delete;
+ HostMemory &operator=(const HostMemory &) = delete;
+
+ virtual ~HostMemory() { CHECK_CUDA(cudaFreeHost(span_.data())); }
+
+ // Returns a pointer to the memory.
+ T *get() { return span_.data(); }
+ const T *get() const { return span_.data(); }
+
+ // Returns the number of objects the memory can hold.
+ size_t size() const { return span_.size(); }
+
+ // Copies data from other (host memory) to this's memory.
+ void MemcpyFrom(const T *other) {
+ memcpy(span_.data(), other, sizeof(T) * size());
+ }
+ // Copies data to other (host memory) from this's memory.
+ void MemcpyTo(const T *other) {
+ memcpy(other, span_.data(), sizeof(T) * size());
+ }
+
+ private:
+ std::span<T> span_;
+};
+
+// Class to manage the lifetime of device memory.
+template <typename T>
+class GpuMemory {
+ public:
+ // Allocates a block of memory for holding up to size objects of type T in
+ // device memory.
+ GpuMemory(size_t size) : size_(size) {
+ CHECK_CUDA(cudaMalloc((void **)(&memory_), size * sizeof(T)));
+ }
+ GpuMemory(const GpuMemory &) = delete;
+ GpuMemory &operator=(const GpuMemory &) = delete;
+
+ virtual ~GpuMemory() { CHECK_CUDA(cudaFree(memory_)); }
+
+ // Returns the device pointer to the memory.
+ T *get() { return memory_; }
+ const T *get() const { return memory_; }
+
+ // Returns the number of objects this memory can hold.
+ size_t size() const { return size_; }
+
+ // Copies data from host memory to this memory asynchronously on the provided
+ // stream.
+ void MemcpyAsyncFrom(const T *host_memory, CudaStream *stream) {
+ CHECK_CUDA(cudaMemcpyAsync(memory_, host_memory, sizeof(T) * size_,
+ cudaMemcpyHostToDevice, stream->get()));
+ }
+ void MemcpyAsyncFrom(const HostMemory<T> *host_memory, CudaStream *stream) {
+ MemcpyAsyncFrom(host_memory->get(), stream);
+ }
+
+ // Copies data to host memory from this memory asynchronously on the provided
+ // stream.
+ void MemcpyAsyncTo(T *host_memory, CudaStream *stream) const {
+ CHECK_CUDA(cudaMemcpyAsync(reinterpret_cast<void *>(host_memory),
+ reinterpret_cast<void *>(memory_),
+ sizeof(T) * size_, cudaMemcpyDeviceToHost,
+ stream->get()));
+ }
+ void MemcpyAsyncTo(HostMemory<T> *host_memory, CudaStream *stream) const {
+ MemcpyAsyncTo(host_memory->get(), stream);
+ }
+
+ // Copies data from host_memory to this memory blocking.
+ void MemcpyFrom(const T *host_memory) {
+ CHECK_CUDA(cudaMemcpy(reinterpret_cast<void *>(memory_),
+ reinterpret_cast<const void *>(host_memory),
+ sizeof(T) * size_, cudaMemcpyHostToDevice));
+ }
+ void MemcpyFrom(const HostMemory<T> *host_memory) {
+ MemcpyFrom(host_memory->get());
+ }
+
+ // Copies data to host_memory from this memory. Only copies size objects.
+ void MemcpyTo(T *host_memory, size_t size) const {
+ CHECK_CUDA(cudaMemcpy(reinterpret_cast<void *>(host_memory), memory_,
+ sizeof(T) * size, cudaMemcpyDeviceToHost));
+ }
+ // Copies data to host_memory from this memory.
+ void MemcpyTo(T *host_memory) const { MemcpyTo(host_memory, size_); }
+ void MemcpyTo(HostMemory<T> *host_memory) const {
+ MemcpyTo(host_memory->get());
+ }
+
+ // Sets the memory asynchronously to contain data of type 'val' on the provide
+ // stream.
+ void MemsetAsync(const uint8_t val, CudaStream *stream) const {
+ CHECK_CUDA(cudaMemsetAsync(memory_, val, sizeof(T) * size_, stream->get()));
+ }
+
+ // Allocates a vector on the host, copies size objects into it, and returns
+ // it.
+ std::vector<T> Copy(size_t s) const {
+ CHECK_LE(s, size_);
+ std::vector<T> result(s);
+ MemcpyTo(result.data(), s);
+ return result;
+ }
+
+ // Copies all the objects in this memory to a vector on the host and returns
+ // it.
+ std::vector<T> Copy() const { return Copy(size_); }
+
+ private:
+ T *memory_;
+ const size_t size_;
+};
+
+// Synchronizes and CHECKs for success the last CUDA operation.
+void CheckAndSynchronize();
+
+// Synchronizes and CHECKS iff --sync is passed on the command line. Makes it
+// so we can leave debugging in the code.
+void MaybeCheckAndSynchronize();
+
+} // namespace apriltag
+} // namespace frc971
+
+#endif // FRC971_ORIN_CUDA_H_
diff --git a/frc971/orin/cuda_april_tag_test.cc b/frc971/orin/cuda_april_tag_test.cc
new file mode 100644
index 0000000..776caf5
--- /dev/null
+++ b/frc971/orin/cuda_april_tag_test.cc
@@ -0,0 +1,1537 @@
+#include <numeric>
+#include <random>
+#include <string>
+
+#include "glog/logging.h"
+#include "gtest/gtest.h"
+#include "opencv2/imgproc.hpp"
+#include "third_party/apriltag/apriltag.h"
+#include "third_party/apriltag/common/unionfind.h"
+#include "third_party/apriltag/tag16h5.h"
+#include <opencv2/highgui.hpp>
+
+#include "aos/flatbuffer_merge.h"
+#include "aos/json_to_flatbuffer.h"
+#include "aos/testing/path.h"
+#include "aos/testing/random_seed.h"
+#include "aos/time/time.h"
+#include "frc971/orin/apriltag.h"
+#include "frc971/vision/vision_generated.h"
+
+DEFINE_int32(pixel_border, 10,
+ "Size of image border within which to reject detected corners");
+DEFINE_double(min_decision_margin, 50.0,
+ "Minimum decision margin (confidence) for an apriltag detection");
+
+DEFINE_bool(debug, false, "If true, write debug images.");
+
+// Get access to the intermediates of aprilrobotics.
+extern "C" {
+
+image_u8_t *threshold(apriltag_detector_t *td, image_u8_t *im);
+unionfind_t *connected_components(apriltag_detector_t *td, image_u8_t *threshim,
+ int w, int h, int ts);
+
+zarray_t *gradient_clusters(apriltag_detector_t *td, image_u8_t *threshim,
+ int w, int h, int ts, unionfind_t *uf);
+
+struct pt {
+ // Note: these represent 2*actual value.
+ uint16_t x, y;
+ int16_t gx, gy;
+
+ float slope;
+};
+} // extern C
+
+// Converts a cv::Mat to an aprilrobotics image.
+image_u8_t ToImageu8t(const cv::Mat &img) {
+ return image_u8_t{
+ .width = img.cols,
+ .height = img.rows,
+ .stride = img.cols,
+ .buf = img.data,
+ };
+}
+
+namespace frc971::apriltag::testing {
+
+// Checks that 2 images match.
+void CheckImage(image_u8_t compare_im_one, image_u8_t compare_im_two,
+ std::string_view label) {
+ CHECK_EQ(compare_im_one.width, compare_im_two.width);
+ CHECK_EQ(compare_im_one.height, compare_im_two.height);
+ ssize_t p = 0;
+ for (int j = 0; j < compare_im_one.height; ++j) {
+ for (int i = 0; i < compare_im_one.width; ++i) {
+ if (p < 0) {
+ LOG(INFO) << i << " " << j << ": "
+ << static_cast<int>(
+ compare_im_one.buf[j * compare_im_one.stride + i])
+ << " (address + " << j * compare_im_one.stride + i << ") vs "
+ << static_cast<int>(
+ compare_im_two.buf[j * compare_im_two.stride + i])
+ << " (address + " << j * compare_im_two.stride + i << ") for "
+ << label;
+
+ ++p;
+ }
+
+ CHECK_EQ(compare_im_one.buf[j * compare_im_one.stride + i],
+ compare_im_two.buf[j * compare_im_two.stride + i])
+ << "First Image Value "
+ << (int)compare_im_one.buf[j * compare_im_one.stride + i] << " "
+ << "Second Image Value "
+ << (int)compare_im_two.buf[j * compare_im_two.stride + i] << " "
+ << "At " << i << ", " << j << " for " << label;
+ }
+ }
+}
+
+// Stores info about a blob.
+struct BlobInfo {
+ size_t size;
+ size_t min_x;
+ size_t min_y;
+ size_t max_x;
+ size_t max_y;
+ size_t april_robotics_id;
+ size_t bounding_area() const { return (max_x - min_x) * (max_y - min_y); }
+};
+
+// Checks that the aprilrobotics and GPU code agree on the results of
+// unionfinding.
+std::map<uint32_t, BlobInfo> CheckUnionfind(unionfind_t *uf,
+ cv::Mat union_markers,
+ const uint32_t *union_markers_size,
+ cv::Mat threshold) {
+ const size_t width = union_markers.cols;
+ const size_t height = union_markers.rows;
+ std::map<uint32_t, uint32_t> id_remap;
+ std::map<uint32_t, size_t> cuda_id_count;
+
+ for (size_t y = 0; y < height; ++y) {
+ for (size_t x = 0; x < width; ++x) {
+ uint32_t v = unionfind_get_representative(uf, y * width + x);
+ uint32_t v_remapped;
+ {
+ auto it = cuda_id_count.emplace(union_markers.at<uint32_t>(y, x), 1);
+ if (!it.second) {
+ ++it.first->second;
+ }
+ }
+ auto it = id_remap.find(v);
+ if (it == id_remap.end()) {
+ v_remapped = union_markers.at<uint32_t>(y, x);
+ id_remap.insert(std::make_pair(v, v_remapped));
+ } else {
+ v_remapped = it->second;
+ }
+
+ CHECK_EQ(v_remapped, union_markers.at<uint32_t>(y, x))
+ << "At " << x << ", " << y;
+ }
+ }
+ for (auto [key, value] : cuda_id_count) {
+ VLOG(2) << "Found " << key << " num times " << value;
+ }
+ LOG(INFO) << "Found " << id_remap.size() << " blob ids in aprilrobotics.";
+
+ std::map<uint32_t, BlobInfo> blob_sizes;
+
+ for (size_t y = 0; y < height; ++y) {
+ for (size_t x = 0; x < width; ++x) {
+ auto it = blob_sizes.emplace(
+ union_markers.at<uint32_t>(y, x),
+ BlobInfo{
+ .size = 1,
+ .min_x = x,
+ .min_y = y,
+ .max_x = x,
+ .max_y = y,
+ .april_robotics_id =
+ unionfind_get_representative(uf, y * width + x),
+ });
+ if (!it.second) {
+ BlobInfo *info = &(it.first->second);
+ ++(info->size);
+ CHECK_EQ(info->april_robotics_id,
+ unionfind_get_representative(uf, y * width + x));
+ info->min_x = std::min(info->min_x, x);
+ info->max_x = std::max(info->max_x, x);
+ info->min_y = std::min(info->min_y, y);
+ info->max_y = std::max(info->max_y, y);
+ }
+ }
+ }
+
+ for (size_t y = 0; y < height; ++y) {
+ for (size_t x = 0; x < width; ++x) {
+ const uint32_t blob_id = union_markers.at<uint32_t>(y, x);
+ const BlobInfo &blob_info = blob_sizes[blob_id];
+ if (threshold.at<uint8_t>(y, x) == 127) {
+ CHECK_EQ(0u, union_markers_size[blob_id])
+ << " at (" << x << ", " << y << ") -> " << blob_id;
+ continue;
+ }
+
+ if (blob_info.size >= 25u) {
+ CHECK_LE(25u, union_markers_size[blob_id])
+ << " at (" << x << ", " << y << ") -> " << blob_id;
+ } else {
+ CHECK_EQ(blob_info.size, union_markers_size[blob_id])
+ << " at (" << x << ", " << y << ") -> " << blob_id;
+ }
+ }
+ }
+
+ LOG(INFO) << "Union finding + stats passed.";
+
+ return blob_sizes;
+}
+
+// Makes a tag detector.
+apriltag_detector_t *MakeTagDetector(apriltag_family_t *tag_family) {
+ apriltag_detector_t *tag_detector = apriltag_detector_create();
+
+ apriltag_detector_add_family_bits(tag_detector, tag_family, 1);
+
+ tag_detector->nthreads = 1;
+ tag_detector->wp = workerpool_create(tag_detector->nthreads);
+ tag_detector->qtp.min_white_black_diff = 5;
+ tag_detector->debug = FLAGS_debug;
+
+ return tag_detector;
+}
+
+class CudaAprilTagDetector {
+ public:
+ CudaAprilTagDetector(size_t width, size_t height)
+ : tag_family_(tag16h5_create()),
+ tag_detector_(MakeTagDetector(tag_family_)),
+ gray_cuda_(cv::Size(width, height), CV_8UC1),
+ decimated_cuda_(gray_cuda_.size() / 2, CV_8UC1),
+ thresholded_cuda_(decimated_cuda_.size(), CV_8UC1),
+ union_markers_(decimated_cuda_.size(), CV_32SC1),
+ union_markers_size_(decimated_cuda_.size(), CV_32SC1),
+ gpu_detector_(width, height, tag_detector_),
+ width_(width),
+ height_(height) {
+ // Report out info about our GPU.
+ {
+ cudaDeviceProp prop;
+ CHECK_EQ(cudaGetDeviceProperties(&prop, 0), cudaSuccess);
+
+ LOG(INFO) << "Device: sm_" << prop.major << prop.minor;
+#define DUMP(x) LOG(INFO) << "" #x ": " << prop.x;
+ DUMP(sharedMemPerBlock);
+ DUMP(l2CacheSize);
+ DUMP(maxThreadsPerBlock);
+ DUMP(maxThreadsPerMultiProcessor);
+ DUMP(memoryBusWidth);
+ DUMP(memoryClockRate);
+ DUMP(multiProcessorCount);
+ DUMP(maxBlocksPerMultiProcessor);
+ DUMP(name);
+ DUMP(warpSize);
+
+#undef DUMP
+ }
+
+ union_marker_pair_.resize((width - 2) / 2 * (height - 2) / 2 * 4,
+ QuadBoundaryPoint());
+ compressed_union_marker_pair_.resize((width - 2) / 2 * (height - 2) / 2 * 4,
+ QuadBoundaryPoint());
+
+ // Pre-compute the border and width thresholds.
+ for (int i = 0; i < zarray_size(tag_detector_->tag_families); i++) {
+ apriltag_family_t *family;
+ zarray_get(tag_detector_->tag_families, i, &family);
+ if (family->width_at_border < min_tag_width_) {
+ min_tag_width_ = family->width_at_border;
+ }
+ normal_border_ |= !family->reversed_border;
+ reversed_border_ |= family->reversed_border;
+ }
+ min_tag_width_ /= tag_detector_->quad_decimate;
+ if (min_tag_width_ < 3) {
+ min_tag_width_ = 3;
+ }
+ }
+
+ ~CudaAprilTagDetector() {
+ apriltag_detector_destroy(tag_detector_);
+ free(tag_family_);
+ }
+
+ // Detects tags on the GPU.
+ void DetectGPU(cv::Mat color_image) {
+ CHECK_EQ(color_image.size(), gray_cuda_.size());
+
+ gpu_detector_.Detect(color_image.data);
+
+ gpu_detector_.CopyGrayTo(gray_cuda_.data);
+ gpu_detector_.CopyDecimatedTo(decimated_cuda_.data);
+ gpu_detector_.CopyThresholdedTo(thresholded_cuda_.data);
+
+ gpu_detector_.CopyUnionMarkersTo(
+ reinterpret_cast<uint32_t *>(union_markers_.data));
+ gpu_detector_.CopyUnionMarkersSizeTo(
+ reinterpret_cast<uint32_t *>(union_markers_size_.data));
+
+ gpu_detector_.CopyUnionMarkerPairTo(union_marker_pair_.data());
+ gpu_detector_.CopyCompressedUnionMarkerPairTo(
+ compressed_union_marker_pair_.data());
+ sorted_union_marker_pair_ = gpu_detector_.CopySortedUnionMarkerPair();
+ num_compressed_union_marker_pair_ =
+ gpu_detector_.NumCompressedUnionMarkerPairs();
+ extents_cuda_ = gpu_detector_.CopyExtents();
+ reduced_dot_blobs_pair_cuda_ = gpu_detector_.CopyReducedDotBlobs();
+ selected_blobs_cuda_ = gpu_detector_.CopySelectedBlobs();
+ sorted_selected_blobs_cuda_ = gpu_detector_.CopySortedSelectedBlobs();
+ num_quads_ = gpu_detector_.NumQuads();
+
+ LOG(INFO) << "num_compressed_union_marker_pair "
+ << sorted_union_marker_pair_.size();
+ }
+
+ // Detects tags on the CPU.
+ void DetectCPU(cv::Mat color_image) {
+ cv::Mat gray_image(color_image.size(), CV_8UC1);
+ cv::cvtColor(color_image, gray_image, cv::COLOR_YUV2GRAY_YUYV);
+ image_u8_t im = {
+ .width = gray_image.cols,
+ .height = gray_image.rows,
+ .stride = gray_image.cols,
+ .buf = gray_image.data,
+ };
+
+ LOG(INFO) << "Starting CPU detect.";
+ zarray_t *detections = apriltag_detector_detect(tag_detector_, &im);
+
+ timeprofile_display(tag_detector_->tp);
+
+ for (int i = 0; i < zarray_size(detections); i++) {
+ apriltag_detection_t *det;
+ zarray_get(detections, i, &det);
+
+ if (det->decision_margin > FLAGS_min_decision_margin) {
+ VLOG(1) << "Found tag number " << det->id
+ << " hamming: " << det->hamming
+ << " margin: " << det->decision_margin;
+ }
+ }
+ }
+
+ // Checks that the union finding algorithms agree.
+ std::vector<QuadBoundaryPoint> CheckUnionMarkerPairs(
+ const std::map<uint32_t, BlobInfo> &blob_sizes) const {
+ std::vector<QuadBoundaryPoint> expected_union_marker_pair;
+
+ const size_t width = thresholded_cuda_.cols;
+ const size_t height = thresholded_cuda_.rows;
+
+ expected_union_marker_pair.resize((width - 2) * (height - 2) * 4,
+ QuadBoundaryPoint());
+
+ LOG(INFO) << "Width: " << width << ", Height " << height;
+
+ auto ToIndex = [&](size_t x, size_t y) {
+ return x - 1 + (y - 1) * (width - 2);
+ };
+
+ size_t wrong = 0;
+ size_t right_nonzero = 0;
+
+ for (size_t y = 1; y < height - 1; ++y) {
+ for (size_t x = 1; x < width - 1; ++x) {
+ const uint8_t v0 = thresholded_cuda_.at<uint8_t>(y, x);
+ const uint64_t rep0 = union_markers_.at<uint32_t>(y, x);
+ const auto blob0 = blob_sizes.find(rep0)->second;
+
+ size_t offset = 0;
+ for (auto [dx, dy] : {std::pair<ssize_t, ssize_t>{1, 0},
+ std::pair<ssize_t, ssize_t>{1, 1},
+ std::pair<ssize_t, ssize_t>{0, 1},
+ std::pair<ssize_t, ssize_t>{-1, 1}}) {
+ const uint8_t v1 = thresholded_cuda_.at<uint8_t>(y + dy, x + dx);
+ const uint32_t rep1 = union_markers_.at<uint32_t>(y + dy, x + dx);
+ const auto blob1 = blob_sizes.find(rep1)->second;
+
+ // OK, blob 1, 1 is special. We only do it if blocks 2 and the blob
+ // let of x won't be connected.
+ // ________
+ // | x | 0 |
+ // -------------
+ // | 3 | 2 | 1 |
+ // -------------
+ //
+ // We are fine checking this all here since we verify that we agree
+ // with aprilrobotics at the end to make sure the overall logic
+ // matches.
+ bool consider_blob = true;
+ if (offset == 3) {
+ const uint8_t v1_block_left =
+ thresholded_cuda_.at<uint8_t>(y + 0, x - 1);
+ const uint32_t rep1_block_left =
+ union_markers_.at<uint32_t>(y + 0, x - 1);
+ const uint8_t v1_block_2 =
+ thresholded_cuda_.at<uint8_t>(y + 1, x + 0);
+ const uint32_t rep1_block_2 =
+ union_markers_.at<uint32_t>(y + 1, x + 0);
+ if (v1_block_left != 127 && v1_block_2 != 127 &&
+ (v1_block_left + v1_block_2) == 255 &&
+ blob_sizes.find(rep1_block_left)->second.size >= 25 &&
+ blob_sizes.find(rep1_block_2)->second.size >= 25 && x != 1) {
+ consider_blob = false;
+ }
+ }
+
+ QuadBoundaryPoint point;
+ if (consider_blob && blob0.size >= 25 && blob1.size >= 25 &&
+ v0 != 127) {
+ if (v0 + v1 == 255) {
+ if (rep0 < rep1) {
+ point.set_rep1(rep1);
+ point.set_rep0(rep0);
+ } else {
+ point.set_rep1(rep0);
+ point.set_rep0(rep1);
+ }
+ point.set_base_xy(x, y);
+ point.set_dxy(offset);
+ point.set_black_to_white(v1 > v0);
+ }
+ }
+ size_t index = ToIndex(x, y) + offset * (width - 2) * (height - 2);
+
+ QuadBoundaryPoint actual = union_marker_pair_[index];
+ if (!point.near(actual)) {
+ CHECK_LT(wrong, 10u);
+ LOG(WARNING) << "point == actual (" << std::hex << point << ", "
+ << actual << ") : Failed at (" << std::dec << x << ", "
+ << y << ") + (" << dx << ", " << dy
+ << "), v0: " << static_cast<int>(v0)
+ << ", v1: " << static_cast<int>(v1)
+ << ", rep0: " << std::hex << rep0 << " rep1: " << rep1
+ << " rep0 size: " << std::dec << blob0.size
+ << " rep1 size: " << std::dec << blob1.size
+ << " consider_blob " << consider_blob;
+ ++wrong;
+ } else if (point.nonzero()) {
+ right_nonzero++;
+ VLOG(2) << "point == actual (" << std::hex << point << ", "
+ << actual << ") : Success at (" << std::dec << x << ", "
+ << y << ") + (" << dx << ", " << dy
+ << "), v0: " << static_cast<int>(v0)
+ << ", v1: " << static_cast<int>(v1)
+ << ", rep0: " << std::hex << rep0 << " rep1: " << rep1;
+ point = actual;
+ }
+ expected_union_marker_pair[index] = point;
+
+ ++offset;
+ }
+ }
+ }
+ CHECK_EQ(wrong, 0u) << ", got " << right_nonzero << " right";
+
+ for (size_t i = 0; i < expected_union_marker_pair.size(); ++i) {
+ CHECK_EQ(expected_union_marker_pair[i], union_marker_pair_[i]);
+ }
+
+ return expected_union_marker_pair;
+ }
+
+ // Checks that the compressed marker pairs GPU algorithm agrees with a naive
+ // CPU version. Returns the expected points.
+ std::vector<QuadBoundaryPoint> CheckCompressedUnionMarkerPairs(
+ const std::vector<QuadBoundaryPoint> &expected_union_marker_pair,
+ const std::vector<QuadBoundaryPoint> &compressed_union_marker_pair,
+ const std::vector<QuadBoundaryPoint> &sorted_union_marker_pair) const {
+ std::vector<QuadBoundaryPoint> expected_compressed_union_marker_pair;
+ {
+ size_t nonzero_count = 0;
+ for (const QuadBoundaryPoint x : expected_union_marker_pair) {
+ if (x.nonzero()) {
+ ++nonzero_count;
+ }
+ }
+ // Rip out all the zeros.
+ expected_compressed_union_marker_pair.reserve(nonzero_count);
+ for (const QuadBoundaryPoint x : expected_union_marker_pair) {
+ if (x.nonzero()) {
+ expected_compressed_union_marker_pair.push_back(x);
+ }
+ }
+
+ CHECK_EQ(static_cast<size_t>(sorted_union_marker_pair.size()),
+ expected_compressed_union_marker_pair.size());
+
+ // And make sure they match.
+ for (size_t i = 0; i < expected_compressed_union_marker_pair.size();
+ ++i) {
+ CHECK_EQ(expected_compressed_union_marker_pair[i],
+ compressed_union_marker_pair[i]);
+ }
+ }
+
+ // Now, sort the points.
+ std::sort(expected_compressed_union_marker_pair.begin(),
+ expected_compressed_union_marker_pair.end(),
+ [&](const QuadBoundaryPoint a, const QuadBoundaryPoint b) {
+ if (a.rep01() != b.rep01()) {
+ return a.rep01() < b.rep01();
+ }
+
+ return a < b;
+ });
+
+ for (size_t i = 0; i < static_cast<size_t>(sorted_union_marker_pair.size());
+ ++i) {
+ CHECK_EQ(expected_compressed_union_marker_pair[i].rep01(),
+ sorted_union_marker_pair[i].rep01());
+ }
+
+ return expected_compressed_union_marker_pair;
+ }
+
+ // Sorts the list of points by slope and blob ID.
+ std::vector<QuadBoundaryPoint> SlopeSortPoints(
+ std::vector<QuadBoundaryPoint> points) const {
+ std::map<uint64_t, std::pair<float, float>> blob_centers;
+
+ // The slope algorithm used by aprilrobotics.
+ auto ComputeSlope = [&blob_centers](QuadBoundaryPoint pair) -> float {
+ const int32_t x = pair.x();
+ const int32_t y = pair.y();
+
+ auto blob_center = blob_centers.find(pair.rep01());
+ CHECK(blob_center != blob_centers.end());
+
+ const float cx = blob_center->second.first;
+ const float cy = blob_center->second.second;
+
+ float quadrants[2][2] = {{-1 * (2 << 15), 0}, {2 * (2 << 15), 2 << 15}};
+
+ float dx = x - cx;
+ float dy = y - cy;
+
+ float quadrant = quadrants[dy > 0][dx > 0];
+ if (dy < 0) {
+ dy = -dy;
+ dx = -dx;
+ }
+
+ if (dx < 0) {
+ float tmp = dx;
+ dx = dy;
+ dy = -tmp;
+ }
+
+ return quadrant + dy / dx;
+ };
+
+ // The theta algorithm used by cuda.
+ auto ComputeTheta = [&blob_centers](QuadBoundaryPoint pair) -> long double {
+ const int32_t x = pair.x();
+ const int32_t y = pair.y();
+
+ auto blob_center = blob_centers.find(pair.rep01());
+ CHECK(blob_center != blob_centers.end());
+
+ const float cx = blob_center->second.first;
+ const float cy = blob_center->second.second;
+
+ float dx = x - cx;
+ float dy = y - cy;
+
+ // make atan2 more accurate than cuda to correctly sort
+ return atan2f64(dy, dx);
+ };
+
+ for (size_t i = 0; i < points.size();) {
+ uint64_t first_rep01 = points[i].rep01();
+
+ int min_x, min_y, max_x, max_y;
+ min_x = max_x = points[i].x();
+ min_y = max_y = points[i].y();
+
+ size_t j = i;
+ for (; j < points.size() && points[j].rep01() == first_rep01; ++j) {
+ QuadBoundaryPoint pt = points[j];
+
+ int x = pt.x();
+ int y = pt.y();
+ min_x = std::min(min_x, x);
+ max_x = std::max(max_x, x);
+ min_y = std::min(min_y, y);
+ max_y = std::max(max_y, y);
+ }
+
+ const float cx = (max_x + min_x) * 0.5 + 0.05118;
+ const float cy = (max_y + min_y) * 0.5 + -0.028581;
+
+ blob_centers[first_rep01] = std::make_pair(cx, cy);
+ i = j;
+ }
+
+ std::sort(points.begin(), points.end(),
+ [&](const QuadBoundaryPoint a, const QuadBoundaryPoint b) {
+ if (a.rep01() != b.rep01()) {
+ return a.rep01() < b.rep01();
+ }
+
+ const float slopea = ComputeSlope(a);
+ const float slopeb = ComputeSlope(b);
+
+ // Sigh, apparently the slope algorithm isn't great and
+ // sometimes ends up with matching slopes. In that case,
+ // compute the actual angle to the X axis too and sort with it.
+ //
+ // Aprilrobotics ignores this case and ends up with an
+ // indeterminate solution. We don't, so we notice.
+ if (slopea == slopeb) {
+ return ComputeTheta(a) < ComputeTheta(b);
+ }
+ return slopea < slopeb;
+ });
+ return points;
+ }
+
+ // Filters blobs using the the various size thresholds that aprilrobotics
+ // uses.
+ std::vector<std::vector<QuadBoundaryPoint>> FilterBlobs(
+ std::vector<std::vector<QuadBoundaryPoint>> blobs) const {
+ std::vector<std::vector<QuadBoundaryPoint>> result;
+ const size_t max_april_tag_perimeter = 2 * (width_ + height_);
+ LOG(ERROR) << "Max permiter test " << max_april_tag_perimeter;
+
+ for (std::vector<QuadBoundaryPoint> &blob : blobs) {
+ int min_x, min_y, max_x, max_y;
+ min_x = max_x = blob[0].x();
+ min_y = max_y = blob[0].y();
+
+ for (const QuadBoundaryPoint pt : blob) {
+ int x = pt.x();
+ int y = pt.y();
+ min_x = std::min(min_x, x);
+ max_x = std::max(max_x, x);
+ min_y = std::min(min_y, y);
+ max_y = std::max(max_y, y);
+ }
+
+ float dot = 0;
+
+ const float cx = (min_x + max_x) * 0.5 + 0.05118;
+ const float cy = (min_y + max_y) * 0.5 - 0.028581;
+
+ for (size_t j = 0; j < blob.size(); ++j) {
+ dot += (static_cast<float>(blob[j].x()) - cx) * blob[j].gx() +
+ (static_cast<float>(blob[j].y()) - cy) * blob[j].gy();
+ }
+
+ const bool quad_reversed_border = dot < 0;
+
+ if (!reversed_border_ && quad_reversed_border) {
+ continue;
+ }
+ if (!normal_border_ && !quad_reversed_border) {
+ continue;
+ }
+ if ((max_x - min_x) * (max_y - min_y) < min_tag_width_) {
+ continue;
+ }
+ if (blob.size() < 24) {
+ continue;
+ }
+ if (blob.size() <
+ static_cast<size_t>(tag_detector_->qtp.min_cluster_pixels)) {
+ continue;
+ }
+ if (blob.size() > max_april_tag_perimeter) {
+ continue;
+ }
+
+ result.emplace_back(std::move(blob));
+ }
+
+ return result;
+ }
+
+ // Produces sorted lists of clusters from AprilRobotics' intermediate
+ // clusters.
+ std::vector<std::vector<uint64_t>> AprilRoboticsPoints(
+ image_u8_t *thresholded_im, unionfind_t *uf) const {
+ zarray_t *clusters =
+ gradient_clusters(tag_detector_, thresholded_im, thresholded_im->width,
+ thresholded_im->height, thresholded_im->stride, uf);
+
+ std::vector<std::vector<uint64_t>> april_grouped_points;
+ for (int i = 0; i < zarray_size(clusters); i++) {
+ zarray_t *cluster;
+ zarray_get(clusters, i, &cluster);
+
+ uint16_t min_x = std::numeric_limits<uint16_t>::max();
+ uint16_t max_x = 0;
+ uint16_t min_y = std::numeric_limits<uint16_t>::max();
+ uint16_t max_y = 0;
+ std::vector<std::tuple<float, uint64_t>> pts;
+ for (int j = 0; j < zarray_size(cluster); j++) {
+ struct pt *p;
+ zarray_get_volatile(cluster, j, &p);
+ min_x = std::min(p->x, min_x);
+ min_y = std::min(p->y, min_y);
+ max_x = std::max(p->x, max_x);
+ max_y = std::max(p->y, max_y);
+ }
+
+ // add some noise to (cx,cy) so that pixels get a more diverse set
+ // of theta estimates. This will help us remove more points.
+ // (Only helps a small amount. The actual noise values here don't
+ // matter much at all, but we want them [-1, 1]. (XXX with
+ // fixed-point, should range be bigger?)
+ const float cx = (min_x + max_x) * 0.5 + 0.05118;
+ const float cy = (min_y + max_y) * 0.5 + -0.028581;
+
+ float quadrants[2][2] = {{-1 * (2 << 15), 0}, {2 * (2 << 15), 2 << 15}};
+
+ for (int j = 0; j < zarray_size(cluster); j++) {
+ struct pt *p;
+ zarray_get_volatile(cluster, j, &p);
+
+ float dx = p->x - cx;
+ float dy = p->y - cy;
+
+ float quadrant = quadrants[dy > 0][dx > 0];
+ if (dy < 0) {
+ dy = -dy;
+ dx = -dx;
+ }
+
+ if (dx < 0) {
+ float tmp = dx;
+ dx = dy;
+ dy = -tmp;
+ }
+ p->slope = quadrant + dy / dx;
+ pts.push_back(std::make_pair(p->slope, p->x + p->y * width_));
+ }
+
+ // Slope sort doesn't always combine duplicates because of duplicate
+ // slopes. Sort by point first, remove duplicates, then sort by slope.
+ std::sort(
+ pts.begin(), pts.end(),
+ [](std::tuple<float, uint64_t> a, std::tuple<float, uint64_t> b) {
+ return std::get<1>(a) < std::get<1>(b);
+ });
+ pts.erase(std::unique(pts.begin(), pts.end(),
+ [](std::tuple<float, uint64_t> a,
+ std::tuple<float, uint64_t> b) {
+ return std::get<1>(a) == std::get<1>(b);
+ }),
+ pts.end());
+
+ std::sort(
+ pts.begin(), pts.end(),
+ [](std::tuple<float, uint64_t> a, std::tuple<float, uint64_t> b) {
+ return std::get<0>(a) < std::get<0>(b);
+ });
+
+ VLOG(1) << "aprilrobotics points of " << pts.size() << " center (" << cx
+ << ", " << cy << ")";
+
+ std::vector<uint64_t> transformed_points;
+ transformed_points.reserve(pts.size());
+ for (std::tuple<float, uint64_t> pt : pts) {
+ VLOG(1) << " (" << std::get<1>(pt) % width_ << ", "
+ << std::get<1>(pt) / width_ << ") slope " << std::get<0>(pt);
+ transformed_points.push_back(std::get<1>(pt));
+ }
+
+ april_grouped_points.emplace_back(std::move(transformed_points));
+ }
+
+ // Now sort the groups of points into a reproducible result by sorting by
+ // size and then smallest point first.
+ std::sort(april_grouped_points.begin(), april_grouped_points.end(),
+ [](auto &x, auto &y) {
+ if (x.size() == y.size()) {
+ for (size_t j = 0; j < x.size(); ++j) {
+ if (x[j] == y[j]) {
+ continue;
+ }
+ return x[j] < y[j];
+ }
+ LOG(FATAL) << "Equal";
+ }
+ return x.size() < y.size();
+ });
+
+ for (int i = 0; i < zarray_size(clusters); i++) {
+ zarray_t *cluster;
+ zarray_get(clusters, i, &cluster);
+ zarray_destroy(cluster);
+ }
+ zarray_destroy(clusters);
+
+ return april_grouped_points;
+ }
+
+ // Sorts point lists by size. These points need to have an x() and y().
+ template <typename T>
+ std::vector<std::vector<T>> SortPointSizes(
+ std::vector<std::vector<T>> in) const {
+ std::sort(in.begin(), in.end(), [this](auto &a, auto &b) {
+ if (a.size() == b.size()) {
+ // Use point ID as the tie breaker.
+ for (size_t j = 0; j < a.size(); ++j) {
+ const uint32_t point_x = a[j].x() + a[j].y() * width_;
+ const uint32_t point_y = b[j].x() + b[j].y() * width_;
+ if (point_x == point_y) {
+ continue;
+ }
+ return point_x < point_y;
+ }
+ LOG(FATAL) << "Equal";
+ }
+ return a.size() < b.size();
+ });
+
+ return in;
+ }
+
+ // Sorts grouped points by size and point.
+ std::vector<std::vector<QuadBoundaryPoint>> SortPoints(
+ std::vector<std::vector<QuadBoundaryPoint>> in) const {
+ for (std::vector<QuadBoundaryPoint> &grouped_points : in) {
+ std::sort(grouped_points.begin(), grouped_points.end(),
+ [this](const QuadBoundaryPoint &a, const QuadBoundaryPoint &b) {
+ return std::make_tuple(a.rep01(), a.x() + a.y() * width_) <
+ std::make_tuple(b.rep01(), b.x() + b.y() * width_);
+ });
+ }
+
+ return SortPointSizes(std::move(in));
+ }
+
+ // Sorts outer points list by size and point.
+ std::vector<std::vector<uint64_t>> SortAprilroboticsPointSizes(
+ std::vector<std::vector<uint64_t>> april_grouped_points) const {
+ std::sort(april_grouped_points.begin(), april_grouped_points.end(),
+ [](auto &x, auto &y) {
+ if (x.size() == y.size()) {
+ for (size_t j = 0; j < x.size(); ++j) {
+ if (x[j] == y[j]) {
+ continue;
+ }
+ return x[j] < y[j];
+ }
+ LOG(FATAL) << "Equal";
+ }
+ return x.size() < y.size();
+ });
+ return april_grouped_points;
+ }
+
+ // Sorts points and points lists for Aprilrobotics by size and point id.
+ std::vector<std::vector<uint64_t>> SortAprilroboticsPoints(
+ std::vector<std::vector<uint64_t>> april_grouped_points) const {
+ for (std::vector<uint64_t> &points : april_grouped_points) {
+ std::sort(points.begin(), points.end());
+ }
+ return SortAprilroboticsPointSizes(april_grouped_points);
+ }
+
+ // Prints out a list of CUDA points.
+ void PrintCudaPoints(
+ const std::vector<std::vector<QuadBoundaryPoint>> &cuda_grouped_points,
+ const std::map<uint32_t, BlobInfo> &blob_sizes) const {
+ for (const std::vector<QuadBoundaryPoint> &points : cuda_grouped_points) {
+ const uint32_t rep0 = points[0].rep0();
+ const uint32_t rep1 = points[0].rep1();
+ VLOG(1) << "CUDA points of " << rep0 << "+" << rep1
+ << " aprilrobotics blob: "
+ << blob_sizes.find(rep0)->second.april_robotics_id << "+"
+ << blob_sizes.find(rep1)->second.april_robotics_id << " ("
+ << std::hex
+ << ((static_cast<uint64_t>(std::max(
+ blob_sizes.find(rep0)->second.april_robotics_id,
+ blob_sizes.find(rep1)->second.april_robotics_id))
+ << 32) |
+ std::min(blob_sizes.find(rep0)->second.april_robotics_id,
+ blob_sizes.find(rep1)->second.april_robotics_id))
+ << std::dec << ") size " << points.size();
+ for (const QuadBoundaryPoint point : points) {
+ VLOG(1) << " (" << point.x() << ", " << point.y() << ")";
+ }
+ }
+
+ LOG(INFO) << "Found runs overall " << cuda_grouped_points.size();
+ }
+
+ // Groups marker pairs by runs of rep01().
+ std::vector<std::vector<QuadBoundaryPoint>> CudaGroupedPoints(
+ const std::vector<QuadBoundaryPoint> &compressed_union_marker_pairs)
+ const {
+ std::vector<std::vector<QuadBoundaryPoint>> cuda_grouped_points;
+ cuda_grouped_points.emplace_back();
+
+ QuadBoundaryPoint start = compressed_union_marker_pairs[0];
+ for (size_t i = 0; i < compressed_union_marker_pairs.size(); ++i) {
+ QuadBoundaryPoint current = compressed_union_marker_pairs[i];
+ CHECK_GE(current.rep01(), start.rep01())
+ << " Failed on index " << i << " of "
+ << compressed_union_marker_pairs.size();
+ if ((start.rep01()) != (current.rep01())) {
+ cuda_grouped_points.emplace_back(
+ std::vector<QuadBoundaryPoint>{current});
+ } else {
+ cuda_grouped_points.back().emplace_back(current);
+ }
+ start = current;
+ }
+
+ return cuda_grouped_points;
+ }
+
+ // Groups marker pairs by runs of rep01().
+ std::vector<std::vector<IndexPoint>> CudaGroupedPoints(
+ const std::vector<IndexPoint> &compressed_cuda_points) const {
+ std::vector<std::vector<IndexPoint>> cuda_grouped_points;
+ cuda_grouped_points.emplace_back();
+
+ IndexPoint start = compressed_cuda_points[0];
+ for (size_t i = 0; i < compressed_cuda_points.size(); ++i) {
+ IndexPoint current = compressed_cuda_points[i];
+ CHECK_GE(current.blob_index(), start.blob_index())
+ << " Failed on index " << i << " of "
+ << compressed_cuda_points.size();
+ if ((start.blob_index()) != (current.blob_index())) {
+ cuda_grouped_points.emplace_back(std::vector<IndexPoint>{current});
+ } else {
+ cuda_grouped_points.back().emplace_back(current);
+ }
+ start = current;
+ }
+
+ return cuda_grouped_points;
+ }
+
+ // Checks that both april robotics and Cuda agree on point grouping. No
+ // ordering is implied here, that is checked later.
+ void CheckAprilRoboticsAndCudaContainSamePoints(
+ const std::vector<std::vector<uint64_t>> &april_grouped_points,
+ const std::vector<std::vector<QuadBoundaryPoint>> &cuda_grouped_points)
+ const {
+ const std::vector<std::vector<QuadBoundaryPoint>>
+ cuda_point_sorted_grouped_points = SortPoints(cuda_grouped_points);
+ const std::vector<std::vector<uint64_t>> april_sorted_grouped_points =
+ SortAprilroboticsPoints(
+ SortAprilroboticsPointSizes(april_grouped_points));
+
+ CHECK_EQ(april_sorted_grouped_points.size(),
+ cuda_point_sorted_grouped_points.size());
+ for (size_t i = 0; i < april_sorted_grouped_points.size(); ++i) {
+ CHECK_EQ(april_sorted_grouped_points[i].size(),
+ cuda_point_sorted_grouped_points[i].size());
+ for (size_t j = 0; j < april_sorted_grouped_points[i].size(); ++j) {
+ CHECK_EQ(april_sorted_grouped_points[i][j],
+ cuda_point_sorted_grouped_points[i][j].x() +
+ cuda_point_sorted_grouped_points[i][j].y() * width_)
+ << ": On list of size " << april_sorted_grouped_points[i].size()
+ << ", failed on point " << j << "("
+ << april_sorted_grouped_points[i][j] % width_ << ", "
+ << april_sorted_grouped_points[i][j] / width_ << ") vs ("
+ << cuda_point_sorted_grouped_points[i][j].x() << ", "
+ << cuda_point_sorted_grouped_points[i][j].y() << ")";
+ }
+ }
+ }
+
+ // Tests that the extents and dot products match the cuda versions.
+ std::pair<size_t, std::vector<IndexPoint>> CheckCudaExtents(
+ const std::vector<std::vector<QuadBoundaryPoint>> &cuda_grouped_points,
+ const std::vector<MinMaxExtents> &extents_cuda,
+ const std::vector<float> &reduced_dot_blobs_pair_cuda) const {
+ std::vector<IndexPoint> selected_blobs;
+ const size_t max_april_tag_perimeter = 2 * (width_ + height_);
+ size_t selected_quads = 0;
+ {
+ BlobExtentsIndexFinder finder(extents_cuda.data(), extents_cuda.size());
+ size_t i = 0;
+ size_t starting_offset = 0;
+ CHECK_EQ(cuda_grouped_points.size(), extents_cuda.size());
+ CHECK_EQ(extents_cuda.size(), reduced_dot_blobs_pair_cuda.size());
+ for (const std::vector<QuadBoundaryPoint> &points : cuda_grouped_points) {
+ size_t min_x, min_y, max_x, max_y;
+ min_x = max_x = points[0].x();
+ min_y = max_y = points[0].y();
+
+ for (const QuadBoundaryPoint pt : points) {
+ size_t x = pt.x();
+ size_t y = pt.y();
+ min_x = std::min(min_x, x);
+ max_x = std::max(max_x, x);
+ min_y = std::min(min_y, y);
+ max_y = std::max(max_y, y);
+ }
+
+ const MinMaxExtents extents = extents_cuda[i];
+ CHECK_EQ(extents.min_x, min_x);
+ CHECK_EQ(extents.max_x, max_x);
+ CHECK_EQ(extents.min_y, min_y);
+ CHECK_EQ(extents.max_y, max_y);
+ CHECK_EQ(extents.count, points.size());
+ CHECK_EQ(extents.starting_offset, starting_offset)
+ << " for index " << i;
+
+ float dot = 0;
+
+ const float cx = (min_x + max_x) * 0.5 + 0.05118;
+ const float cy = (min_y + max_y) * 0.5 - 0.028581;
+
+ bool good_blob_size =
+ points.size() >= 24 && points.size() <= max_april_tag_perimeter &&
+ points.size() >=
+ static_cast<size_t>(tag_detector_->qtp.min_cluster_pixels);
+
+ if (good_blob_size) {
+ for (size_t j = 0; j < points.size(); ++j) {
+ dot += (static_cast<float>(points[j].x()) - cx) * points[j].gx() +
+ (static_cast<float>(points[j].y()) - cy) * points[j].gy();
+
+ // Make sure our blob finder agrees.
+ CHECK_EQ(i, finder.FindBlobIndex(starting_offset + j));
+ }
+ // Test that the summed dot product is right.
+ CHECK_LT(
+ std::abs(reduced_dot_blobs_pair_cuda[i] - dot) / std::abs(dot),
+ 1e-4)
+ << ": for point " << i << ", cuda -> "
+ << reduced_dot_blobs_pair_cuda[i] << ", C++ -> " << dot;
+ } else {
+ CHECK_EQ(reduced_dot_blobs_pair_cuda[i], 0.0f)
+ << ": for point " << i << ", cuda -> "
+ << reduced_dot_blobs_pair_cuda[i] << ", C++ -> " << dot;
+ }
+
+ const bool quad_reversed_border = dot < 0;
+
+ VLOG(1) << "For point " << i << ", cuda -> "
+ << reduced_dot_blobs_pair_cuda[i] << ", C++ -> " << dot
+ << " size " << points.size() << " border "
+ << (!(!reversed_border_ && quad_reversed_border) &&
+ !(!normal_border_ && !quad_reversed_border))
+ << " area: "
+ << (extents.max_x - extents.min_x) *
+ (extents.max_y - extents.min_y)
+ << " min_area: " << min_tag_width_;
+
+ if (good_blob_size && !(!reversed_border_ && quad_reversed_border) &&
+ !(!normal_border_ && !quad_reversed_border)) {
+ ++selected_quads;
+ for (size_t j = 0; j < points.size(); ++j) {
+ IndexPoint pt(i, points[j].point_bits());
+ float theta =
+ (atan2f(pt.y() - extents.cy(), pt.x() - extents.cx()) + M_PI) *
+ 8e6;
+ long long int theta_int = llrintf(theta);
+
+ pt.set_theta(theta_int);
+ selected_blobs.emplace_back(pt);
+ }
+ }
+
+ starting_offset += points.size();
+ ++i;
+ }
+ }
+ return std::make_pair(selected_quads, selected_blobs);
+ }
+
+ // Tests that the filtered points lists are sorted by key.
+ void CheckFilteredCudaPoints(
+ const std::vector<IndexPoint> &selected_blobs,
+ const std::vector<IndexPoint> &selected_blobs_cuda) const {
+ CHECK_EQ(selected_blobs.size(), selected_blobs_cuda.size());
+ for (size_t i = 0;
+ i < std::min(selected_blobs.size(), selected_blobs_cuda.size()); ++i) {
+ VLOG(1) << "Got blob[" << i << "] -> " << selected_blobs[i] << " vs "
+ << selected_blobs_cuda[i];
+ CHECK_EQ(selected_blobs[i].blob_index(),
+ selected_blobs_cuda[i].blob_index());
+ }
+ }
+
+ // Tests that the cuda sorted point list matches C++.
+ void CheckSortedFilteredCudaPoints(
+ const std::vector<std::vector<QuadBoundaryPoint>>
+ &slope_sorted_expected_grouped_points,
+ size_t selected_quads, const std::vector<IndexPoint> &selected_blobs,
+ const std::vector<IndexPoint> &sorted_selected_blobs_cuda) const {
+ CHECK_EQ(selected_blobs.size(), sorted_selected_blobs_cuda.size());
+ const std::vector<std::vector<IndexPoint>> cuda_grouped_points =
+ CudaGroupedPoints(sorted_selected_blobs_cuda);
+
+ CHECK_EQ(cuda_grouped_points.size(), selected_quads);
+ CHECK_EQ(slope_sorted_expected_grouped_points.size(), selected_quads);
+
+ size_t missmatched_points = 0;
+ for (size_t i = 0; i < cuda_grouped_points.size(); ++i) {
+ const std::vector<IndexPoint> &cuda_grouped_blob = cuda_grouped_points[i];
+ const std::vector<QuadBoundaryPoint> &slope_sorted_points =
+ slope_sorted_expected_grouped_points[i];
+
+ CHECK_EQ(cuda_grouped_blob.size(), slope_sorted_points.size());
+ if (VLOG_IS_ON(1) && cuda_grouped_blob[0].blob_index() == 160) {
+ for (size_t j = 0; j < cuda_grouped_points[i].size(); ++j) {
+ LOG(INFO) << "For blob " << cuda_grouped_blob[0].blob_index()
+ << ", got " << cuda_grouped_blob[j] << " ("
+ << cuda_grouped_blob[j].x() << ", "
+ << cuda_grouped_blob[j].y() << ") expected "
+ << slope_sorted_points[j] << " ("
+ << slope_sorted_points[j].x() << ", "
+ << slope_sorted_points[j].y() << ")";
+ }
+ }
+
+ size_t missmatched_runs = 0;
+
+ // recalculates the theta to be used in the following check
+ std::map<uint64_t, std::pair<float, float>> blob_centers;
+ auto ComputeTheta =
+ [&blob_centers](QuadBoundaryPoint pair) -> long double {
+ const int32_t x = pair.x();
+ const int32_t y = pair.y();
+
+ auto blob_center = blob_centers.find(pair.rep01());
+ CHECK(blob_center != blob_centers.end());
+
+ const float cx = blob_center->second.first;
+ const float cy = blob_center->second.second;
+
+ float dx = x - cx;
+ float dy = y - cy;
+
+ return atan2f64(dy, dx);
+ };
+
+ // refinds blob centers
+ for (size_t i = 0; i < slope_sorted_points.size();) {
+ uint64_t first_rep01 = slope_sorted_points[i].rep01();
+
+ int min_x, min_y, max_x, max_y;
+ min_x = max_x = slope_sorted_points[i].x();
+ min_y = max_y = slope_sorted_points[i].y();
+
+ size_t j = i;
+ for (; j < slope_sorted_points.size() &&
+ slope_sorted_points[j].rep01() == first_rep01;
+ ++j) {
+ QuadBoundaryPoint pt = slope_sorted_points[j];
+
+ int x = pt.x();
+ int y = pt.y();
+ min_x = std::min(min_x, x);
+ max_x = std::max(max_x, x);
+ min_y = std::min(min_y, y);
+ max_y = std::max(max_y, y);
+ }
+
+ const float cx = (max_x + min_x) * 0.5 + 0.05118;
+ const float cy = (max_y + min_y) * 0.5 + -0.028581;
+
+ blob_centers[first_rep01] = std::make_pair(cx, cy);
+ i = j;
+ }
+
+ for (size_t j = 0; j < cuda_grouped_points[i].size(); ++j) {
+ if (cuda_grouped_blob[j].x() != slope_sorted_points[j].x() ||
+ cuda_grouped_blob[j].y() != slope_sorted_points[j].y()) {
+ size_t allowable_swapped_indices = 1;
+ long double max_allowed_imprecision = 3e-7;
+ // search through indixes j - a to j + a to see if they're swapped
+ // also only doesn't count if the precision needed to differentiate is
+ // less than max_allowed_imprecision
+ for (size_t k = j - allowable_swapped_indices;
+ k <= j + allowable_swapped_indices; k++) {
+ if (cuda_grouped_blob[j].x() == slope_sorted_points[k].x() &&
+ cuda_grouped_blob[j].y() == slope_sorted_points[k].y() &&
+ abs(ComputeTheta(slope_sorted_points[k]) -
+ ComputeTheta(slope_sorted_points[j])) <
+ max_allowed_imprecision) {
+ continue;
+ }
+ }
+ ++missmatched_points;
+ ++missmatched_runs;
+ // We shouldn't see a lot of points in a row which don't match.
+ VLOG(1) << "Missmatched point in blob "
+ << cuda_grouped_blob[0].blob_index() << ", point " << j
+ << " (" << cuda_grouped_blob[j].x() << ", "
+ << cuda_grouped_blob[j].y() << ") vs ("
+ << slope_sorted_points[j].x() << ", "
+ << slope_sorted_points[j].y() << ")"
+ << " in size " << cuda_grouped_points[i].size();
+ CHECK_LE(missmatched_runs, 4u);
+ } else {
+ missmatched_runs = 0;
+ }
+ }
+ }
+
+ // Or a lot of points overall. The slope algo has duplicate points, and
+ // is occasionally wrong.
+ CHECK_LE(missmatched_points, 50u);
+ }
+
+ // Checks that the GPU and CPU algorithms match.
+ void Check(cv::Mat color_image) {
+ cv::Mat gray_image(color_image.size(), CV_8UC1);
+ cv::cvtColor(color_image, gray_image, cv::COLOR_YUV2GRAY_YUYV);
+
+ image_u8_t gray_im = ToImageu8t(gray_image);
+ CheckImage(gray_im, ToImageu8t(gray_cuda_), "gray_cuda");
+
+ image_u8_t *quad_im =
+ image_u8_decimate(&gray_im, tag_detector_->quad_decimate);
+ CheckImage(*quad_im, ToImageu8t(decimated_cuda_), "decimated_cuda");
+
+ image_u8_t *thresholded_im = threshold(tag_detector_, quad_im);
+ CheckImage(ToImageu8t(thresholded_cuda_), *thresholded_im,
+ "threshold_cuda");
+
+ unionfind_t *uf = connected_components(
+ tag_detector_, thresholded_im, thresholded_im->width,
+ thresholded_im->height, thresholded_im->stride);
+
+ // Checks union finding.
+ auto blob_sizes = CheckUnionfind(
+ uf, union_markers_,
+ reinterpret_cast<const uint32_t *>(union_markers_size_.data),
+ thresholded_cuda_);
+
+ // Now make sure our unfiltered and partially sorted lists of pairs are
+ // plausible.
+ const std::vector<QuadBoundaryPoint> expected_union_marker_pair =
+ CheckUnionMarkerPairs(blob_sizes);
+ const std::vector<QuadBoundaryPoint> expected_compressed_union_marker_pair =
+ CheckCompressedUnionMarkerPairs(expected_union_marker_pair,
+ compressed_union_marker_pair_,
+ sorted_union_marker_pair_);
+
+ const std::vector<std::vector<QuadBoundaryPoint>> cuda_grouped_points =
+ CudaGroupedPoints(sorted_union_marker_pair_);
+ PrintCudaPoints(cuda_grouped_points, blob_sizes);
+
+ const std::vector<std::vector<QuadBoundaryPoint>>
+ slope_sorted_expected_grouped_points = FilterBlobs(CudaGroupedPoints(
+ SlopeSortPoints(expected_compressed_union_marker_pair)));
+
+ const std::vector<std::vector<uint64_t>> april_grouped_points =
+ AprilRoboticsPoints(thresholded_im, uf);
+
+ LOG(INFO) << "Found " << april_grouped_points.size()
+ << " clusters with april robotics with "
+ << std::accumulate(
+ april_grouped_points.begin(), april_grouped_points.end(),
+ 0, [](int size, const auto &v) { return size + v.size(); })
+ << " points.";
+ LOG(INFO) << "Found " << cuda_grouped_points.size()
+ << " clusters with cuda with "
+ << num_compressed_union_marker_pair_ << " points.";
+
+ // Verify that both aprilrobotics and us group points the same. Ignore
+ // order.
+ CheckAprilRoboticsAndCudaContainSamePoints(april_grouped_points,
+ cuda_grouped_points);
+
+ // Test that the extents are reasonable.
+ size_t selected_quads;
+ std::vector<IndexPoint> selected_blobs;
+ std::tie(selected_quads, selected_blobs) = CheckCudaExtents(
+ cuda_grouped_points, extents_cuda_, reduced_dot_blobs_pair_cuda_);
+
+ // And that the filtered list is reasonable.
+ CheckFilteredCudaPoints(selected_blobs, selected_blobs_cuda_);
+
+ // And that they sorted right too.
+ CheckSortedFilteredCudaPoints(slope_sorted_expected_grouped_points,
+ selected_quads, selected_blobs,
+ sorted_selected_blobs_cuda_);
+
+ // TODO(austin): Check the selected extents is right.
+ LOG(INFO) << "Found slope sorted count: "
+ << sorted_union_marker_pair_.size();
+
+ unionfind_destroy(uf);
+ image_u8_destroy(quad_im);
+ image_u8_destroy(thresholded_im);
+ }
+
+ // Writes images out to /tmp for debugging.
+ void WriteDebug(cv::Mat color_image) {
+ cv::Mat bgr(color_image.size(), CV_8UC3);
+ cv::cvtColor(color_image, bgr, cv::COLOR_YUV2BGR_YUYV);
+ cv::imwrite("/tmp/debug_color_image.png", bgr);
+
+ cv::imwrite("/tmp/debug_halide_grey_cuda.png", gray_cuda_);
+ cv::imwrite("/tmp/debug_halide_grey_decimated.png", decimated_cuda_);
+ cv::imwrite("/tmp/debug_cuda_thresholded.png", thresholded_cuda_);
+
+ {
+ image_u8_t thresholded_halide_im = ToImageu8t(thresholded_cuda_);
+
+ unionfind_t *uf = connected_components(
+ tag_detector_, &thresholded_halide_im, thresholded_halide_im.width,
+ thresholded_halide_im.height, thresholded_halide_im.stride);
+
+ std::map<uint32_t, uint32_t> april_to_cuda_id_remap;
+ std::map<uint32_t, uint32_t> cuda_to_april_id_remap;
+
+ std::map<uint32_t, cv::Vec3b> colors;
+
+ // Seed the random number generator so we get the same images out each
+ // time for easier comparison.
+ srandom(0);
+
+ // Write out the union find image.
+ size_t color_count = 1;
+ uint32_t max_color = 0;
+ cv::Mat unionfind_image(union_markers_.size(), CV_8UC3);
+ cv::Mat unionfind_image_common(union_markers_.size(), CV_8UC3);
+ for (int y = 0; y < union_markers_.rows; ++y) {
+ for (int x = 0; x < union_markers_.cols; ++x) {
+ const uint32_t april_robotics_id =
+ unionfind_get_representative(uf, y * width_ / 2 + x);
+ const uint32_t index = union_markers_.at<uint32_t>(y, x);
+
+ bool one_to_one_blob_match = false;
+
+ {
+ auto it = cuda_to_april_id_remap.find(index);
+ if (it == cuda_to_april_id_remap.end()) {
+ // Now, check we don't have a match the other way.
+ auto it_back = april_to_cuda_id_remap.find(april_robotics_id);
+ if (it_back == april_to_cuda_id_remap.end()) {
+ one_to_one_blob_match = true;
+ cuda_to_april_id_remap.emplace(index, april_robotics_id);
+ april_to_cuda_id_remap.emplace(april_robotics_id, index);
+ } else {
+ one_to_one_blob_match = false;
+ }
+ } else {
+ auto it_back = april_to_cuda_id_remap.find(april_robotics_id);
+ if (it_back == april_to_cuda_id_remap.end()) {
+ one_to_one_blob_match = false;
+ } else {
+ if (it->second == april_robotics_id &&
+ it_back->second == index) {
+ one_to_one_blob_match = true;
+ }
+ }
+ }
+ }
+
+ cv::Vec3b color;
+
+ auto it = colors.find(index);
+ if (it == colors.end()) {
+ max_color = std::max(max_color, index);
+ ++color_count;
+ VLOG(2) << "New color 0x" << std::hex << index;
+ const int bias = 50;
+ uint8_t r = bias + (random() % (200 - bias));
+ uint8_t g = bias + (random() % (200 - bias));
+ uint8_t b = bias + (random() % (200 - bias));
+
+ color = cv::Vec3b(b, g, r);
+ colors[index] = color;
+ } else {
+ color = it->second;
+ }
+
+ unionfind_image.at<cv::Vec3b>(y, x) = color;
+
+ if (one_to_one_blob_match) {
+ if (index == static_cast<uint32_t>(x + y * union_markers_.cols)) {
+ color = cv::Vec3b(0, 0, 0);
+ } else if (thresholded_cuda_.at<uint8_t>(y, x) == 0) {
+ color = cv::Vec3b(0, 0, 0);
+ } else {
+ color = cv::Vec3b(255, 255, 255);
+ }
+ }
+
+ CHECK(one_to_one_blob_match);
+
+ unionfind_image_common.at<cv::Vec3b>(y, x) = color;
+ }
+ }
+ LOG(INFO) << "Found " << color_count << " colors with a max index of "
+ << max_color;
+ cv::imwrite("/tmp/debug_cuda_segmentation.png", unionfind_image);
+ cv::imwrite("/tmp/debug_cuda_segmentation_common.png",
+ unionfind_image_common);
+
+ unionfind_destroy(uf);
+ }
+
+ {
+ // And write out the marker pairs image too.
+ std::map<uint64_t, cv::Vec3b> colors;
+
+ cv::Mat sorted_marker_pair(cv::Size((width_), (height_)), CV_8UC3);
+ sorted_marker_pair.setTo(cv::Scalar(0, 0, 0));
+
+ for (size_t i = 0; i < sorted_union_marker_pair_.size();) {
+ size_t blob_size = 0;
+ for (size_t j = i; j < sorted_union_marker_pair_.size(); ++j) {
+ if (sorted_union_marker_pair_[i].rep01() !=
+ sorted_union_marker_pair_[j].rep01()) {
+ break;
+ }
+ ++blob_size;
+ }
+ for (size_t j = i; j < sorted_union_marker_pair_.size(); ++j) {
+ if (sorted_union_marker_pair_[i].rep01() !=
+ sorted_union_marker_pair_[j].rep01()) {
+ break;
+ }
+ QuadBoundaryPoint pair = sorted_union_marker_pair_[j];
+
+ const size_t x = pair.x();
+ const size_t y = pair.y();
+
+ auto it = colors.find(pair.rep01());
+ cv::Vec3b color;
+ if (it == colors.end()) {
+ VLOG(2) << "New color 0x" << std::hex << pair.rep01();
+ const int bias = 50;
+ uint8_t r = bias + (random() % (200 - bias));
+ uint8_t g = bias + (random() % (200 - bias));
+ uint8_t b = bias + (random() % (200 - bias));
+
+ color = cv::Vec3b(b, g, r);
+ colors[pair.rep01()] = color;
+ } else {
+ color = it->second;
+ }
+ sorted_marker_pair.at<cv::Vec3b>(y + 2, x + 2) = color;
+ }
+ i += blob_size;
+ }
+ cv::imwrite("/tmp/debug_cuda_marker_pairs.png", sorted_marker_pair);
+ }
+ }
+
+ private:
+ apriltag_family_t *tag_family_;
+ apriltag_detector_t *tag_detector_;
+
+ cv::Mat gray_cuda_;
+ cv::Mat decimated_cuda_;
+ cv::Mat thresholded_cuda_;
+
+ cv::Mat union_markers_;
+ cv::Mat union_markers_size_;
+
+ std::vector<QuadBoundaryPoint> union_marker_pair_;
+ std::vector<QuadBoundaryPoint> compressed_union_marker_pair_;
+ std::vector<QuadBoundaryPoint> sorted_union_marker_pair_;
+ std::vector<uint32_t> quad_length_;
+ std::vector<MinMaxExtents> extents_cuda_;
+ std::vector<float> reduced_dot_blobs_pair_cuda_;
+ std::vector<IndexPoint> selected_blobs_cuda_;
+ std::vector<IndexPoint> sorted_selected_blobs_cuda_;
+ int num_compressed_union_marker_pair_ = 0;
+ int num_quads_ = 0;
+
+ GpuDetector gpu_detector_;
+
+ size_t width_;
+ size_t height_;
+
+ bool normal_border_ = false;
+ bool reversed_border_ = false;
+ int min_tag_width_ = 1000000;
+};
+
+class AprilDetectionTest : public ::testing::Test {
+ public:
+ aos::FlatbufferVector<frc971::vision::CameraImage> ReadImage(
+ std::string_view path) {
+ return aos::FileToFlatbuffer<frc971::vision::CameraImage>(
+ "../apriltag_test_bfbs_images/" + std::string(path));
+ }
+
+ cv::Mat ToMat(const frc971::vision::CameraImage *image) {
+ cv::Mat color_image(cv::Size(image->cols(), image->rows()), CV_8UC2,
+ (void *)image->data()->data());
+ return color_image;
+ }
+};
+
+TEST_F(AprilDetectionTest, ImageRepeat) {
+ auto image = ReadImage("bfbs-capture-2013-01-18_08-54-09.501047728.bfbs");
+
+ LOG(INFO) << "Image is: " << image.message().cols() << " x "
+ << image.message().rows();
+
+ CudaAprilTagDetector cuda_detector(image.message().cols(),
+ image.message().rows());
+
+ const cv::Mat color_image = ToMat(&image.message());
+
+ for (size_t i = 0; i < 10; ++i) {
+ LOG(INFO) << "Attempt " << i;
+ cuda_detector.DetectGPU(color_image.clone());
+ cuda_detector.DetectCPU(color_image.clone());
+ cuda_detector.Check(color_image.clone());
+ if (FLAGS_debug) {
+ cuda_detector.WriteDebug(color_image);
+ }
+ }
+}
+
+class SingleAprilDetectionTest
+ : public ::testing::WithParamInterface<std::string_view>,
+ public AprilDetectionTest {};
+
+// Tests a single image.
+TEST_P(SingleAprilDetectionTest, Image) {
+ auto image = ReadImage(GetParam());
+
+ LOG(INFO) << "Testing " << GetParam() << " with dimensions "
+ << image.message().cols() << " x " << image.message().rows();
+
+ CudaAprilTagDetector cuda_detector(image.message().cols(),
+ image.message().rows());
+
+ const cv::Mat color_image = ToMat(&image.message());
+
+ cuda_detector.DetectGPU(color_image.clone());
+ cuda_detector.DetectCPU(color_image.clone());
+ cuda_detector.Check(color_image.clone());
+ if (FLAGS_debug) {
+ cuda_detector.WriteDebug(color_image);
+ }
+}
+
+// Tests that both algorithms agree on a bunch of pixels.
+INSTANTIATE_TEST_SUITE_P(
+ CapturedImages, SingleAprilDetectionTest,
+ ::testing::Values("bfbs-capture-2013-01-18_08-54-16.869057537.bfbs",
+ "bfbs-capture-2013-01-18_08-54-09.501047728.bfbs",
+ "bfbs-capture-2013-01-18_08-51-24.861065764.bfbs",
+ "bfbs-capture-2013-01-18_08-52-01.846912552.bfbs",
+ "bfbs-capture-2013-01-18_08-52-33.462848049.bfbs",
+ "bfbs-capture-2013-01-18_08-54-24.931661979.bfbs",
+ "bfbs-capture-2013-01-18_09-29-16.806073486.bfbs",
+ "bfbs-capture-2013-01-18_09-33-00.993756514.bfbs",
+ "bfbs-capture-2013-01-18_08-57-00.171120695.bfbs",
+ "bfbs-capture-2013-01-18_08-57-17.858752817.bfbs",
+ "bfbs-capture-2013-01-18_08-57-08.096597542.bfbs"));
+
+} // namespace frc971::apriltag::testing
diff --git a/frc971/orin/gpu_image.h b/frc971/orin/gpu_image.h
new file mode 100644
index 0000000..732b256
--- /dev/null
+++ b/frc971/orin/gpu_image.h
@@ -0,0 +1,14 @@
+#ifndef FRC971_ORIN_GPU_IMAGE_H_
+#define FRC971_ORIN_GPU_IMAGE_H_
+
+template <typename T>
+struct GpuImage {
+ typedef T type;
+ T *data;
+ size_t rows;
+ size_t cols;
+ // Step is in elements, not bytes.
+ size_t step;
+};
+
+#endif // FRC971_ORIN_GPU_IMAGE_H_
diff --git a/frc971/orin/labeling_allegretti_2019_BKE.cc b/frc971/orin/labeling_allegretti_2019_BKE.cc
new file mode 100644
index 0000000..2806199
--- /dev/null
+++ b/frc971/orin/labeling_allegretti_2019_BKE.cc
@@ -0,0 +1,494 @@
+// Copyright (c) 2020, the YACCLAB contributors, as
+// shown by the AUTHORS file. All rights reserved.
+//
+// Use of this source code is governed by a BSD-style
+// license that can be found in the LICENSE file.
+//
+// See
+// https://iris.unimore.it/bitstream/11380/1179616/1/2018_TPDS_Optimized_Block_Based_Algorithms_to_Label_Connected_Components_on_GPUs.pdf
+// for more details.
+//
+// This file was started from the YACCLAB implementation, but is now quite
+// different to match what the april tag detection algorithm wants. The notable
+// changes are:
+// * 4 way connectivity on the background at the same time as the foreground.
+// * 0 and 255 are the only colors considered, 127 means "I'm my own blob".
+// * Blob size is also computed in parallel using atomics to aid blob size
+// filtering.
+//
+
+#include "frc971/orin/labeling_allegretti_2019_BKE.h"
+
+#include "glog/logging.h"
+
+#include "cuda_runtime.h"
+#include "device_launch_parameters.h"
+
+#define BLOCK_ROWS 16
+#define BLOCK_COLS 16
+
+namespace {
+
+// This is a block-based algorithm.
+// Blocks are 2x2 sized, with internal pixels named as:
+// +---+
+// |a b|
+// |c d|
+// +---+
+//
+// Neighbour blocks of block X are named as:
+// +-+-+-+
+// |P|Q|R|
+// +-+-+-+
+// |S|X|
+// +-+-+
+
+enum class Info : uint8_t {
+ a = 0,
+ b = 1,
+ c = 2,
+ d = 3,
+ P = 4,
+ Q = 5,
+ R = 6,
+ S = 7
+};
+
+// Only use it with unsigned numeric types
+template <typename T>
+__device__ __forceinline__ uint8_t HasBit(const T bitmap, Info pos) {
+ return (bitmap >> static_cast<uint8_t>(pos)) & 1;
+}
+
+template <typename T>
+__device__ __forceinline__ uint8_t HasBit(const T bitmap, uint8_t pos) {
+ return (bitmap >> pos) & 1;
+}
+
+// Only use it with unsigned numeric types
+__device__ __forceinline__ void SetBit(uint8_t &bitmap, Info pos) {
+ bitmap |= (1 << static_cast<uint8_t>(pos));
+}
+
+// Returns the root index of the UFTree
+__device__ uint32_t Find(const uint32_t *s_buf, uint32_t n) {
+ while (s_buf[n] != n) {
+ n = s_buf[n];
+ }
+ return n;
+}
+
+// Returns the root index of the UFTree, re-assigning ourselves as we go.
+__device__ uint32_t FindAndCompress(uint32_t *s_buf, uint32_t n) {
+ uint32_t id = n;
+ while (s_buf[n] != n) {
+ n = s_buf[n];
+ s_buf[id] = n;
+ }
+ return n;
+}
+
+// Merges the UFTrees of a and b, linking one root to the other
+__device__ void Union(uint32_t *s_buf, uint32_t a, uint32_t b) {
+ bool done;
+
+ do {
+ a = Find(s_buf, a);
+ b = Find(s_buf, b);
+
+ if (a < b) {
+ uint32_t old = atomicMin(s_buf + b, a);
+ done = (old == b);
+ b = old;
+ } else if (b < a) {
+ uint32_t old = atomicMin(s_buf + a, b);
+ done = (old == a);
+ a = old;
+ } else {
+ done = true;
+ }
+
+ } while (!done);
+}
+
+// Initializes the labels in an image to hold the masks, info, and UF tree
+// pointers needed for the next steps.
+__global__ void InitLabeling(const GpuImage<uint8_t> img,
+ GpuImage<uint32_t> labels) {
+ const unsigned row = (blockIdx.y * blockDim.y + threadIdx.y) * 2;
+ const unsigned col = (blockIdx.x * blockDim.x + threadIdx.x) * 2;
+ const uint32_t img_index = row * img.step + col;
+ const uint32_t foreground_labels_index = row * labels.step + col;
+ const uint32_t background_labels_index = (row + 1) * labels.step + col;
+
+ if (row >= labels.rows || col >= labels.cols) {
+ return;
+ }
+
+ uint32_t P_foreground = 0;
+ uint32_t P_background = 0;
+
+ // Bitmask representing two kinds of information
+ // Bits 0, 1, 2, 3 are set if pixel a, b, c, d are foreground, respectively
+ // Bits 4, 5, 6, 7 are set if block P, Q, R, S need to be merged to X in
+ // Merge phase
+ uint8_t info_foreground = 0;
+ uint8_t info_left_background = 0;
+ uint8_t info_right_background = 0;
+
+ uint8_t buffer alignas(int)[4];
+ *(reinterpret_cast<int *>(buffer)) = 0;
+
+ // Read pairs of consecutive values in memory at once
+ // This does not depend on endianness
+ *(reinterpret_cast<uint16_t *>(buffer)) =
+ *(reinterpret_cast<uint16_t *>(img.data + img_index));
+
+ *(reinterpret_cast<uint16_t *>(buffer + 2)) =
+ *(reinterpret_cast<uint16_t *>(img.data + img_index + img.step));
+
+ // P is a bitmask saying where to check.
+ //
+ // 0 1 2 3
+ // +----+
+ // 4 |a b| 7
+ // 8 |c d| 11
+ // +----+
+ // 12 13 14 15
+ if (buffer[0] == 255u) {
+ P_foreground |= 0x777;
+ SetBit(info_foreground, Info::a);
+ } else if (buffer[0] == 0u) {
+ // This is the background, we are only doing 4 way connectivity, only look
+ // in the 4 directions.
+ P_background |= 0x272;
+ SetBit(info_left_background, Info::a);
+ }
+
+ if (buffer[1] == 255u) {
+ P_foreground |= (0x777 << 1);
+ SetBit(info_foreground, Info::b);
+ } else if (buffer[1] == 0u) {
+ P_background |= (0x272 << 1);
+ SetBit(info_right_background, Info::b);
+ }
+
+ if (buffer[2] == 255u) {
+ P_foreground |= (0x777 << 4);
+ SetBit(info_foreground, Info::c);
+ } else if (buffer[2] == 0u) {
+ P_background |= (0x272 << 4);
+ SetBit(info_left_background, Info::c);
+ }
+
+ if (buffer[3] == 255u) {
+ SetBit(info_foreground, Info::d);
+ } else if (buffer[3] == 0u) {
+ SetBit(info_right_background, Info::d);
+ }
+
+ if (col == 0) {
+ P_foreground &= 0xEEEE;
+ P_background &= 0xEEEE;
+ }
+ if (col + 2 >= img.cols) {
+ P_foreground &= 0x7777;
+ P_background &= 0x7777;
+ }
+
+ if (row == 0) {
+ P_foreground &= 0xFFF0;
+ P_background &= 0xFFF0;
+ }
+ if (row + 2 >= img.rows) {
+ P_foreground &= 0x0FFF;
+ P_background &= 0x0FFF;
+ }
+
+ // P is now ready to be used to find neighbour blocks
+ // P value avoids range errors
+ int father_offset_foreground = 0;
+ int father_offset_left_background = 0;
+ int father_offset_right_background = 0;
+
+ // P square
+ if (HasBit(P_foreground, 0) && img.data[img_index - img.step - 1] == 255u) {
+ father_offset_foreground = -(2 * (labels.step) + 2);
+ }
+
+ // Q square
+ if ((HasBit(P_foreground, 1) && img.data[img_index - img.step] == 255u) ||
+ (HasBit(P_foreground, 2) && img.data[img_index + 1 - img.step] == 255u)) {
+ if (!father_offset_foreground) {
+ father_offset_foreground = -(2 * (labels.step));
+ } else {
+ SetBit(info_foreground, Info::Q);
+ }
+ }
+ if ((HasBit(P_background, 1) && img.data[img_index - img.step] == 0u)) {
+ father_offset_left_background = -2 * labels.step;
+ }
+ if ((HasBit(P_background, 2) && img.data[img_index + 1 - img.step] == 0u)) {
+ father_offset_right_background = -2 * labels.step;
+ }
+
+ // R square
+ if (HasBit(P_foreground, 3) && img.data[img_index + 2 - img.step] == 255u) {
+ if (!father_offset_foreground) {
+ father_offset_foreground = -(2 * (labels.step) - 2);
+ } else {
+ SetBit(info_foreground, Info::R);
+ }
+ }
+
+ // S square
+ if ((HasBit(P_foreground, 4) && img.data[img_index - 1] == 255u) ||
+ (HasBit(P_foreground, 8) && img.data[img_index + img.step - 1] == 255u)) {
+ if (!father_offset_foreground) {
+ father_offset_foreground = -2;
+ } else {
+ SetBit(info_foreground, Info::S);
+ }
+ }
+ if ((HasBit(P_background, 4) && img.data[img_index - 1] == 0u) ||
+ (HasBit(P_background, 8) && img.data[img_index + img.step - 1] == 0u)) {
+ if (!father_offset_left_background) {
+ father_offset_left_background = -1;
+ } else {
+ SetBit(info_left_background, Info::S);
+ }
+ }
+
+ if ((HasBit(info_left_background, Info::a) &&
+ HasBit(info_right_background, Info::b)) ||
+ (HasBit(info_left_background, Info::c) &&
+ HasBit(info_right_background, Info::d))) {
+ if (!father_offset_right_background) {
+ father_offset_right_background = -1;
+ } else {
+ SetBit(info_right_background, Info::S);
+ }
+ }
+
+ // Now, write everything back out to memory.
+ *reinterpret_cast<uint64_t *>(labels.data + foreground_labels_index) =
+ static_cast<uint64_t>(foreground_labels_index +
+ father_offset_foreground) +
+ (static_cast<uint64_t>(info_foreground) << 32) +
+ (static_cast<uint64_t>(info_left_background) << 40) +
+ (static_cast<uint64_t>(info_right_background) << 48);
+
+ *reinterpret_cast<uint64_t *>(labels.data + background_labels_index) =
+ static_cast<uint64_t>(background_labels_index +
+ father_offset_left_background) +
+ (static_cast<uint64_t>(background_labels_index +
+ father_offset_right_background + 1)
+ << 32);
+}
+
+__global__ void Compression(GpuImage<uint32_t> labels) {
+ const unsigned row = (blockIdx.y * blockDim.y + threadIdx.y) * 2;
+ const unsigned col = (blockIdx.x * blockDim.x + threadIdx.x) * 2;
+ const uint32_t foreground_labels_index = row * labels.step + col;
+ const uint32_t background_labels_index = (row + 1) * labels.step + col;
+
+ if (row >= labels.rows || col >= labels.cols) {
+ return;
+ }
+
+ FindAndCompress(labels.data, foreground_labels_index);
+ FindAndCompress(labels.data, background_labels_index);
+ FindAndCompress(labels.data, background_labels_index + 1);
+}
+
+__global__ void Merge(GpuImage<uint32_t> labels) {
+ const unsigned row = (blockIdx.y * blockDim.y + threadIdx.y) * 2;
+ const unsigned col = (blockIdx.x * blockDim.x + threadIdx.x) * 2;
+ const uint32_t foreground_labels_index = row * labels.step + col;
+ const uint32_t background_labels_index = (row + 1) * labels.step + col;
+
+ if (row >= labels.rows || col >= labels.cols) {
+ return;
+ }
+
+ const uint32_t info = *(labels.data + foreground_labels_index + 1);
+
+ const uint8_t info_foreground = info & 0xff;
+ const uint8_t info_left_background = (info >> 8) & 0xff;
+ const uint8_t info_right_background = (info >> 16) & 0xff;
+
+ if (HasBit(info_foreground, Info::Q)) {
+ Union(labels.data, foreground_labels_index,
+ foreground_labels_index - 2 * (labels.step));
+ }
+
+ if (HasBit(info_foreground, Info::R)) {
+ Union(labels.data, foreground_labels_index,
+ foreground_labels_index - 2 * (labels.step) + 2);
+ }
+
+ if (HasBit(info_foreground, Info::S)) {
+ Union(labels.data, foreground_labels_index, foreground_labels_index - 2);
+ }
+
+ if (HasBit(info_left_background, Info::S)) {
+ Union(labels.data, background_labels_index, background_labels_index - 1);
+ }
+ if (HasBit(info_right_background, Info::S)) {
+ Union(labels.data, background_labels_index + 1, background_labels_index);
+ }
+}
+
+__global__ void FinalLabeling(GpuImage<uint32_t> labels,
+ GpuImage<uint32_t> union_markers_size_device) {
+ const unsigned row = (blockIdx.y * blockDim.y + threadIdx.y) * 2;
+ const unsigned col = (blockIdx.x * blockDim.x + threadIdx.x) * 2;
+ const uint32_t foreground_labels_index = row * labels.step + col;
+ const uint32_t background_labels_index = (row + 1) * labels.step + col;
+
+ if (row >= labels.rows || col >= labels.cols) {
+ return;
+ }
+
+ const uint64_t foreground_buffer =
+ *reinterpret_cast<uint64_t *>(labels.data + foreground_labels_index);
+ const uint32_t foreground_label = (foreground_buffer & (0xFFFFFFFF));
+ const uint8_t foreground_info = (foreground_buffer >> 32) & 0xFF;
+ const uint64_t background_buffer =
+ *reinterpret_cast<uint64_t *>(labels.data + background_labels_index);
+ const uint32_t background_left_label = (background_buffer & (0xFFFFFFFF));
+ const uint32_t background_right_label =
+ ((background_buffer >> 32) & (0xFFFFFFFF));
+ const uint8_t background_left_info = (foreground_buffer >> 40) & 0xFF;
+ const uint8_t background_right_info = (foreground_buffer >> 48) & 0xFF;
+
+ uint32_t a_label;
+ uint32_t b_label;
+ uint32_t c_label;
+ uint32_t d_label;
+
+ if ((foreground_info & 0xf) == 0u && (background_left_info & 0xf) == 0u &&
+ (background_right_info & 0xf) == 0u) {
+ a_label = foreground_labels_index;
+ b_label = foreground_labels_index + 1;
+ c_label = background_labels_index;
+ d_label = background_labels_index + 1;
+ *reinterpret_cast<uint64_t *>(labels.data + foreground_labels_index) =
+ (static_cast<uint64_t>(b_label) << 32) | a_label;
+ *reinterpret_cast<uint64_t *>(labels.data + background_labels_index) =
+ (static_cast<uint64_t>(d_label) << 32) | (c_label);
+ return;
+ } else {
+ a_label = (HasBit(foreground_info, Info::a) * foreground_label +
+ HasBit(background_left_info, Info::a) * background_left_label);
+ b_label = HasBit(foreground_info, Info::b) * foreground_label +
+ HasBit(background_right_info, Info::b) * background_right_label;
+ c_label = HasBit(foreground_info, Info::c) * foreground_label +
+ HasBit(background_left_info, Info::c) * background_left_label;
+ d_label = HasBit(foreground_info, Info::d) * foreground_label +
+ HasBit(background_right_info, Info::d) * background_right_label;
+
+ *reinterpret_cast<uint64_t *>(labels.data + foreground_labels_index) =
+ (static_cast<uint64_t>(b_label) << 32) | a_label;
+ *reinterpret_cast<uint64_t *>(labels.data + background_labels_index) =
+ (static_cast<uint64_t>(d_label) << 32) | (c_label);
+ }
+
+ if ((foreground_info & 0xf) != 0u) {
+ // We've got foreground!
+ size_t count = 0;
+ if (HasBit(foreground_info, Info::a)) {
+ ++count;
+ }
+ if (HasBit(foreground_info, Info::b)) {
+ ++count;
+ }
+ if (HasBit(foreground_info, Info::c)) {
+ ++count;
+ }
+ if (HasBit(foreground_info, Info::d)) {
+ ++count;
+ }
+
+ atomicAdd(union_markers_size_device.data + foreground_label, count);
+ }
+
+ if ((background_left_info & 0xf) == 0u &&
+ (background_right_info & 0xf) == 0u) {
+ return;
+ }
+
+ if ((background_left_info & 0xf) != 0u &&
+ (background_right_info & 0xf) != 0u &&
+ background_left_label == background_right_label) {
+ // They are all populated and match, go for it.
+ size_t count = 0;
+ if (HasBit(background_left_info, Info::a)) {
+ ++count;
+ }
+ if (HasBit(background_right_info, Info::b)) {
+ ++count;
+ }
+ if (HasBit(background_left_info, Info::c)) {
+ ++count;
+ }
+ if (HasBit(background_right_info, Info::d)) {
+ ++count;
+ }
+
+ atomicAdd(union_markers_size_device.data + background_left_label, count);
+ return;
+ }
+
+ if ((background_left_info & 0xf) != 0u) {
+ size_t count = 0;
+ if (HasBit(background_left_info, Info::a)) {
+ ++count;
+ }
+ if (HasBit(background_left_info, Info::c)) {
+ ++count;
+ }
+ atomicAdd(union_markers_size_device.data + background_left_label, count);
+ }
+
+ if ((background_right_info & 0xf) != 0u) {
+ size_t count = 0;
+ if (HasBit(background_right_info, Info::b)) {
+ ++count;
+ }
+ if (HasBit(background_right_info, Info::d)) {
+ ++count;
+ }
+ atomicAdd(union_markers_size_device.data + background_right_label, count);
+ }
+}
+
+} // namespace
+
+void LabelImage(const GpuImage<uint8_t> input, GpuImage<uint32_t> output,
+ GpuImage<uint32_t> union_markers_size_device,
+ cudaStream_t stream) {
+ CHECK_NE(input.rows, 1u);
+ CHECK_NE(input.cols, 1u);
+
+ // Need an even number of rows and colums, we don't need to solve the actual
+ // hard problems...
+ CHECK_EQ(input.rows % 2, 0u);
+ CHECK_EQ(input.cols % 2, 0u);
+
+ dim3 grid_size =
+ dim3((((input.cols + 1) / 2) + BLOCK_COLS - 1) / BLOCK_COLS,
+ (((input.rows + 1) / 2) + BLOCK_ROWS - 1) / BLOCK_ROWS, 1);
+ dim3 block_size = dim3(BLOCK_COLS, BLOCK_ROWS, 1);
+
+ InitLabeling<<<grid_size, block_size, 0, stream>>>(input, output);
+
+ Compression<<<grid_size, block_size, 0, stream>>>(output);
+
+ Merge<<<grid_size, block_size, 0, stream>>>(output);
+
+ Compression<<<grid_size, block_size, 0, stream>>>(output);
+
+ FinalLabeling<<<grid_size, block_size, 0, stream>>>(
+ output, union_markers_size_device);
+}
diff --git a/frc971/orin/labeling_allegretti_2019_BKE.h b/frc971/orin/labeling_allegretti_2019_BKE.h
new file mode 100644
index 0000000..4c87f7f
--- /dev/null
+++ b/frc971/orin/labeling_allegretti_2019_BKE.h
@@ -0,0 +1,12 @@
+#ifndef FRC971_ORIN_LABELING_ALLEGRETTI_2019_BKE_H_
+#define FRC971_ORIN_LABELING_ALLEGRETTI_2019_BKE_H_
+
+#include <cuda_runtime.h>
+
+#include "frc971/orin/gpu_image.h"
+
+void LabelImage(const GpuImage<uint8_t> input, GpuImage<uint32_t> output,
+ GpuImage<uint32_t> union_markers_size_device,
+ cudaStream_t stream);
+
+#endif // FRC971_ORIN_LABELING_ALLEGRETTI_2019_BKE_H_
diff --git a/frc971/orin/output_iterator_test.cc b/frc971/orin/output_iterator_test.cc
new file mode 100644
index 0000000..d4087d0
--- /dev/null
+++ b/frc971/orin/output_iterator_test.cc
@@ -0,0 +1,72 @@
+#include <random>
+
+#include "gtest/gtest.h"
+
+#include "aos/testing/random_seed.h"
+#include "frc971/orin/transform_output_iterator.h"
+
+namespace frc971::apriltag::testing {
+
+struct Mul2 {
+ uint64_t operator()(const uint32_t num) const {
+ return static_cast<uint64_t>(num) * 2;
+ }
+};
+
+// Tests that the transform output iterator both transforms and otherwise acts
+// like a normal pointer
+TEST(TransformOutputIteratorTest, IntArr) {
+ std::mt19937 generator(aos::testing::RandomSeed());
+ std::uniform_int_distribution<uint32_t> random_uint32(0, UINT32_MAX);
+
+ uint32_t *nums_in = (uint32_t *)malloc(UINT32_WIDTH * 20);
+ uint64_t *nums_out = (uint64_t *)malloc(UINT64_WIDTH * 20);
+ uint64_t *expected_out = (uint64_t *)malloc(UINT64_WIDTH * 20);
+
+ for (size_t i = 0; i < 20; i++) {
+ nums_in[i] = random_uint32(generator);
+ expected_out[i] = 2 * static_cast<uint64_t>(nums_in[i]);
+ }
+
+ Mul2 convert_op;
+ TransformOutputIterator<uint32_t, uint64_t, Mul2> itr(nums_out, convert_op);
+
+ // check indirection, array index, increments, and decrements
+ EXPECT_EQ(itr == itr, true);
+ EXPECT_EQ(itr != itr, false);
+ *itr = *nums_in; // [0]
+ *(++itr) = *(++nums_in); // [1]
+ auto temp = itr;
+ auto temp2 = itr++;
+ EXPECT_EQ(temp, temp2); // [2]
+ EXPECT_NE(temp, itr);
+ EXPECT_NE(temp2, itr);
+ nums_in++; // [2]
+ *itr = *nums_in; // [2]
+ auto temp3 = ++itr;
+ auto temp4 = itr;
+ EXPECT_EQ(temp3, temp4); // [3]
+ EXPECT_EQ(temp3, itr);
+ itr--; // [2]
+ auto temp5 = --itr;
+ auto temp6 = itr;
+ EXPECT_EQ(temp5, temp6); // [1]
+ EXPECT_EQ(temp5, itr);
+ nums_in--; // [1]
+ auto temp7 = itr;
+ auto temp8 = itr--;
+ EXPECT_EQ(temp7, temp8); // [0]
+ EXPECT_NE(temp7, itr);
+ EXPECT_NE(temp8, itr);
+ nums_in--; // [0]
+
+ for (size_t i = 3; i < 20; i++) {
+ itr[i] = nums_in[i]; // [3] -> [19]
+ }
+
+ // check expected out and converted out
+ for (size_t i = 0; i < 20; i++) {
+ EXPECT_EQ(expected_out[i], nums_out[i]);
+ }
+}
+} // namespace frc971::apriltag::testing
diff --git a/frc971/orin/points.cc b/frc971/orin/points.cc
new file mode 100644
index 0000000..dbf5496
--- /dev/null
+++ b/frc971/orin/points.cc
@@ -0,0 +1,35 @@
+#include "frc971/orin/points.h"
+
+#include <iomanip>
+#include <ostream>
+
+namespace frc971 {
+namespace apriltag {
+
+std::ostream &operator<<(std::ostream &os, const QuadBoundaryPoint &point) {
+ std::ios_base::fmtflags original_flags = os.flags();
+
+ os << "key:" << std::hex << std::setw(16) << std::setfill('0') << point.key
+ << " rep01:" << std::setw(10) << point.rep01() << " pt:" << std::setw(6)
+ << point.point_bits();
+ os.flags(original_flags);
+ return os;
+}
+
+static_assert(sizeof(QuadBoundaryPoint) == 8,
+ "QuadBoundaryPoint didn't pack right.");
+
+std::ostream &operator<<(std::ostream &os, const IndexPoint &point) {
+ std::ios_base::fmtflags original_flags = os.flags();
+
+ os << "key:" << std::hex << std::setw(16) << std::setfill('0') << point.key
+ << " i:" << std::setw(3) << point.blob_index() << " t:" << std::setw(7)
+ << point.theta() << " p:" << std::setw(6) << point.point_bits();
+ os.flags(original_flags);
+ return os;
+}
+
+static_assert(sizeof(IndexPoint) == 8, "IndexPoint didn't pack right.");
+
+} // namespace apriltag
+} // namespace frc971
diff --git a/frc971/orin/points.h b/frc971/orin/points.h
new file mode 100644
index 0000000..d530a17
--- /dev/null
+++ b/frc971/orin/points.h
@@ -0,0 +1,303 @@
+#ifndef FRC971_ORIN_POINTS_H_
+#define FRC971_ORIN_POINTS_H_
+
+#include <stdint.h>
+
+#include <cub/iterator/transform_input_iterator.cuh>
+#include <cuda/std/tuple>
+#include <iomanip>
+#include <ostream>
+
+#include "cuda_runtime.h"
+#include "device_launch_parameters.h"
+
+namespace frc971 {
+namespace apriltag {
+
+// Class to hold the 2 adjacent blob IDs, a point in decimated image space, the
+// half pixel offset, and the gradient.
+//
+// rep0 and rep1 are the two blob ids, and are each allocated 20 bits.
+// point is the base point and is allocated 10 bits for x and 10 bits for y.
+// dx and dy are allocated 2 bits, and can only take on set values.
+// black_to_white captures the direction of the gradient in 1 bit.
+//
+// This adds up to 63 bits so we can load this with one big load.
+struct QuadBoundaryPoint {
+ static constexpr size_t kRepEndBit = 24;
+ static constexpr size_t kBitsInKey = 64;
+
+ __forceinline__ __host__ __device__ QuadBoundaryPoint() : key(0) {}
+
+ // Sets rep0, the 0th blob id. This only respects the bottom 20 bits.
+ __forceinline__ __host__ __device__ void set_rep0(uint32_t rep0) {
+ key = (key & 0xfffff00000ffffffull) |
+ (static_cast<uint64_t>(rep0 & 0xfffff) << 24);
+ }
+ // Returns rep0.
+ __forceinline__ __host__ __device__ uint32_t rep0() const {
+ return ((key >> 24) & 0xfffff);
+ }
+
+ // Sets rep1, the 1st blob id. This only respects the bottom 20 bits.
+ __forceinline__ __host__ __device__ void set_rep1(uint32_t rep1) {
+ key = (key & 0xfffffffffffull) |
+ (static_cast<uint64_t>(rep1 & 0xfffff) << 44);
+ }
+ // Returns rep1.
+ __forceinline__ __host__ __device__ uint32_t rep1() const {
+ return ((key >> 44) & 0xfffff);
+ }
+
+ // Returns both rep0 and rep1 concatenated into a single 40 bit number.
+ __forceinline__ __host__ __device__ uint64_t rep01() const {
+ return ((key >> 24) & 0xffffffffff);
+ }
+
+ // Returns all the bits used to hold position and gradient information.
+ __forceinline__ __host__ __device__ uint32_t point_bits() const {
+ return key & 0xffffff;
+ }
+
+ // Sets the 10 bit x and y.
+ __forceinline__ __host__ __device__ void set_base_xy(uint32_t x, uint32_t y) {
+ key = (key & 0xffffffffff00000full) |
+ (static_cast<uint64_t>(x & 0x3ff) << 14) |
+ (static_cast<uint64_t>(y & 0x3ff) << 4);
+ }
+
+ // Returns the base 10 bit x and y.
+ __forceinline__ __host__ __device__ uint32_t base_x() const {
+ return ((key >> 14) & 0x3ff);
+ }
+ __forceinline__ __host__ __device__ uint32_t base_y() const {
+ return ((key >> 4) & 0x3ff);
+ }
+
+ // Sets dxy, the integer representing which of the 4 search directions we
+ // went.
+ __forceinline__ __host__ __device__ void set_dxy(uint64_t dxy) {
+ key = (key & 0xfffffffffffffffcull) | (static_cast<uint64_t>(dxy & 0x3));
+ }
+
+ // Returns the change in x derived from the search direction.
+ __forceinline__ __host__ __device__ int32_t dx() const {
+ switch (key & 0x3) {
+ case 0:
+ return 1;
+ case 1:
+ return 1;
+ case 2:
+ return 0;
+ case 3:
+ return -1;
+ }
+ return 0;
+ }
+
+ // Returns the change in y derived from the search direction.
+ __forceinline__ __host__ __device__ int32_t dy() const {
+ switch (key & 0x3) {
+ case 0:
+ return 0;
+ case 1:
+ case 2:
+ case 3:
+ return 1;
+ }
+ return 0;
+ }
+
+ // Returns the un-decimated x and y positions.
+ __forceinline__ __host__ __device__ uint32_t x() const {
+ return static_cast<int32_t>(base_x() * 2) + dx();
+ }
+ __forceinline__ __host__ __device__ uint32_t y() const {
+ return static_cast<int32_t>(base_y() * 2) + dy();
+ }
+
+ // Returns the gradient that this point represents, taking into account which
+ // direction the color transitioned.
+ __forceinline__ __host__ __device__ int8_t gx() const {
+ return black_to_white() ? dx() : -dx();
+ }
+ __forceinline__ __host__ __device__ int8_t gy() const {
+ return black_to_white() ? dy() : -dy();
+ }
+
+ // Returns the black to white or white to black bit.
+ __forceinline__ __host__ __device__ void set_black_to_white(
+ bool black_to_white) {
+ key = (key & 0xfffffffffffffff7ull) |
+ (static_cast<uint64_t>(black_to_white) << 3);
+ }
+ __forceinline__ __host__ __device__ bool black_to_white() const {
+ return (key & 0x8) != 0;
+ }
+
+ // Various operators to make it easy to compare points.
+ __forceinline__ __host__ __device__ bool operator!=(
+ const QuadBoundaryPoint other) const {
+ return other.key != key;
+ }
+ __forceinline__ __host__ __device__ bool operator==(
+ const QuadBoundaryPoint other) const {
+ return other.key == key;
+ }
+ __forceinline__ __host__ __device__ bool operator<(
+ const QuadBoundaryPoint other) const {
+ return key < other.key;
+ }
+
+ // Returns true if this point has been set. Zero is reserved for "invalid"
+ __forceinline__ __host__ __device__ bool nonzero() const {
+ return key != 0ull;
+ }
+
+ // Returns true if this point is about the other point.
+ bool near(QuadBoundaryPoint other) const { return other == *this; }
+
+ // The key. This shouldn't be parsed directly.
+ uint64_t key;
+};
+
+std::ostream &operator<<(std::ostream &os, const QuadBoundaryPoint &point);
+
+// Holds a compacted blob index, the angle to the X axis from the center of the
+// blob, and the coordinate of the point.
+//
+// The blob index is 12 bits, the angle is 28 bits, and the point is 24 bits.
+struct IndexPoint {
+ // Max number of blob IDs we can hold.
+ static constexpr size_t kMaxBlobs = 2048;
+
+ static constexpr size_t kRepEndBit = 24;
+ static constexpr size_t kBitsInKey = 64;
+
+ __forceinline__ __host__ __device__ IndexPoint() : key(0) {}
+
+ // Constructor to build a point with just the blob index, and point bits. The
+ // point bits should be grabbed from a QuadBoundaryPoint rather than built up
+ // by hand.
+ __forceinline__ __host__ __device__ IndexPoint(uint32_t blob_index,
+ uint32_t point_bits)
+ : key((static_cast<uint64_t>(blob_index & 0xfff) << 52) |
+ (static_cast<uint64_t>(point_bits & 0xffffff))) {}
+
+ // Sets and gets the 12 bit blob index.
+ __forceinline__ __host__ __device__ void set_blob_index(uint32_t blob_index) {
+ key = (key & 0x000fffffffffffffull) |
+ (static_cast<uint64_t>(blob_index & 0xfff) << 52);
+ }
+ __forceinline__ __host__ __device__ uint32_t blob_index() const {
+ return ((key >> 52) & 0xfff);
+ }
+
+ // Sets and gets the 28 bit angle.
+ __forceinline__ __host__ __device__ void set_theta(uint32_t theta) {
+ key = (key & 0xfff0000000ffffffull) |
+ (static_cast<uint64_t>(theta & 0xfffffff) << 24);
+ }
+ __forceinline__ __host__ __device__ uint32_t theta() const {
+ return ((key >> 24) & 0xfffffff);
+ }
+
+ // See QuadBoundaryPoint for a description of the rest of these.
+ // Sets the 10 bit x and y.
+ __forceinline__ __host__ __device__ void set_base_xy(uint32_t x, uint32_t y) {
+ key = (key & 0xffffffffff00000full) |
+ (static_cast<uint64_t>(x & 0x3ff) << 14) |
+ (static_cast<uint64_t>(y & 0x3ff) << 4);
+ }
+
+ __forceinline__ __host__ __device__ uint32_t base_x() const {
+ return ((key >> 14) & 0x3ff);
+ }
+
+ __forceinline__ __host__ __device__ uint32_t base_y() const {
+ return ((key >> 4) & 0x3ff);
+ }
+
+ __forceinline__ __host__ __device__ void set_dxy(uint64_t dxy) {
+ key = (key & 0xfffffffffffffffcull) | (static_cast<uint64_t>(dxy & 0x3));
+ }
+
+ __forceinline__ __host__ __device__ int32_t dx() const {
+ switch (key & 0x3) {
+ case 0:
+ return 1;
+ case 1:
+ return 1;
+ case 2:
+ return 0;
+ case 3:
+ return -1;
+ }
+ return 0;
+ }
+
+ __forceinline__ __host__ __device__ int32_t dy() const {
+ switch (key & 0x3) {
+ case 0:
+ return 0;
+ case 1:
+ case 2:
+ case 3:
+ return 1;
+ }
+ return 0;
+ }
+
+ __forceinline__ __host__ __device__ uint32_t x() const {
+ return static_cast<int32_t>(base_x() * 2) + dx();
+ }
+ __forceinline__ __host__ __device__ uint32_t y() const {
+ return static_cast<int32_t>(base_y() * 2) + dy();
+ }
+
+ __forceinline__ __host__ __device__ int8_t gx() const {
+ return black_to_white() ? dx() : -dx();
+ }
+ __forceinline__ __host__ __device__ int8_t gy() const {
+ return black_to_white() ? dy() : -dy();
+ }
+
+ __forceinline__ __host__ __device__ uint32_t point_bits() const {
+ return key & 0xffffff;
+ }
+
+ __forceinline__ __host__ __device__ void set_black_to_white(
+ bool black_to_white) {
+ key = (key & 0xfffffffffffffff7ull) |
+ (static_cast<uint64_t>(black_to_white) << 3);
+ }
+ __forceinline__ __host__ __device__ bool black_to_white() const {
+ return (key & 0x8) != 0;
+ }
+
+ // The key. This shouldn't be parsed directly.
+ uint64_t key;
+};
+
+std::ostream &operator<<(std::ostream &os, const IndexPoint &point);
+
+// Decomposer for sorting which just returns the key.
+struct QuadBoundaryPointDecomposer {
+ __host__ __device__ ::cuda::std::tuple<uint64_t &> operator()(
+ QuadBoundaryPoint &key) const {
+ return {key.key};
+ }
+};
+
+// Decomposer for sorting which just returns the key.
+struct QuadIndexPointDecomposer {
+ __host__ __device__ ::cuda::std::tuple<uint64_t &> operator()(
+ IndexPoint &key) const {
+ return {key.key};
+ }
+};
+
+} // namespace apriltag
+} // namespace frc971
+
+#endif // FRC971_ORIN_POINTS_H_
diff --git a/frc971/orin/points_test.cc b/frc971/orin/points_test.cc
new file mode 100644
index 0000000..852a1e4
--- /dev/null
+++ b/frc971/orin/points_test.cc
@@ -0,0 +1,204 @@
+#include "frc971/orin/points.h"
+
+#include <random>
+
+#include "gtest/gtest.h"
+
+#include "aos/testing/random_seed.h"
+
+namespace frc971::apriltag::testing {
+
+// Tests that QuadBoundaryPoint doesn't corrupt data.
+TEST(QuadBoundaryPoint, MasksWork) {
+ std::mt19937 generator(aos::testing::RandomSeed());
+ std::uniform_int_distribution<uint32_t> random_rep_scalar(0, 0xfffff);
+ std::uniform_int_distribution<uint32_t> random_point_scalar(0, 0x3ff);
+ std::uniform_int_distribution<uint32_t> random_dxy_scalar(0, 3);
+ std::uniform_int_distribution<uint32_t> random_bool(0, 1);
+
+ QuadBoundaryPoint point;
+
+ EXPECT_EQ(point.key, 0);
+
+ for (int i = 0; i < 25; ++i) {
+ const uint32_t rep0 = random_rep_scalar(generator);
+ for (int j = 0; j < 25; ++j) {
+ const uint32_t rep1 = random_rep_scalar(generator);
+ for (int k = 0; k < 25; ++k) {
+ const uint32_t x = random_point_scalar(generator);
+ const uint32_t y = random_point_scalar(generator);
+ for (int k = 0; k < 25; ++k) {
+ const uint32_t dxy = random_dxy_scalar(generator);
+ for (int m = 0; m < 2; ++m) {
+ const bool black_to_white = random_bool(generator) == 1;
+
+ if (point.rep0() != rep0) {
+ point.set_rep0(rep0);
+ }
+
+ if (point.rep1() != rep1) {
+ point.set_rep1(rep1);
+ }
+
+ if (point.base_x() != x || point.base_y() != y) {
+ point.set_base_xy(x, y);
+ }
+
+ switch (dxy) {
+ case 0:
+ if (point.dx() != 1 || point.dy() != 0) {
+ point.set_dxy(dxy);
+ }
+ break;
+ case 1:
+ if (point.dx() != 1 || point.dy() != 1) {
+ point.set_dxy(dxy);
+ }
+ break;
+ case 2:
+ if (point.dx() != 0 || point.dy() != 1) {
+ point.set_dxy(dxy);
+ }
+ break;
+ case 3:
+ if (point.dx() != -1 || point.dy() != 1) {
+ point.set_dxy(dxy);
+ }
+ break;
+ }
+
+ if (black_to_white != point.black_to_white()) {
+ point.set_black_to_white(black_to_white);
+ }
+
+ EXPECT_EQ(point.rep0(), rep0);
+ EXPECT_EQ(point.rep1(), rep1);
+ EXPECT_EQ(point.base_x(), x);
+ EXPECT_EQ(point.base_y(), y);
+ switch (dxy) {
+ case 0:
+ EXPECT_EQ(point.dx(), 1);
+ EXPECT_EQ(point.dy(), 0);
+ break;
+ case 1:
+ EXPECT_EQ(point.dx(), 1);
+ EXPECT_EQ(point.dy(), 1);
+ break;
+ case 2:
+ EXPECT_EQ(point.dx(), 0);
+ EXPECT_EQ(point.dy(), 1);
+ break;
+ case 3:
+ EXPECT_EQ(point.dx(), -1);
+ EXPECT_EQ(point.dy(), 1);
+ break;
+ }
+
+ EXPECT_EQ(point.x(), x * 2 + point.dx());
+ EXPECT_EQ(point.y(), y * 2 + point.dy());
+
+ EXPECT_EQ(point.black_to_white(), black_to_white);
+ }
+ }
+ }
+ }
+ }
+}
+
+// Tests that IndexPoint doesn't corrupt anything
+TEST(IndexPoint, MasksWork) {
+ std::mt19937 generator(
+ aos::testing::RandomSeed()); // random_uint32(generator)
+ std::uniform_int_distribution<uint32_t> random_blob_index(0, 0xfff);
+ std::uniform_int_distribution<uint32_t> random_theta(0, 0xfffffff);
+ std::uniform_int_distribution<uint32_t> random_point_scalar(0, 0x3ff);
+ std::uniform_int_distribution<uint32_t> random_dxy_scalar(0, 3);
+ std::uniform_int_distribution<uint32_t> random_bool(0, 1);
+
+ IndexPoint point;
+
+ for (int i = 0; i < 25; i++) {
+ const uint32_t blob_index = random_blob_index(generator);
+ for (int j = 0; j < 25; j++) {
+ const uint32_t theta = random_theta(generator);
+ for (int k = 0; k < 25; ++k) {
+ const uint32_t x = random_point_scalar(generator);
+ const uint32_t y = random_point_scalar(generator);
+ for (int k = 0; k < 25; ++k) {
+ const uint32_t dxy = random_dxy_scalar(generator);
+ for (int m = 0; m < 2; ++m) {
+ const bool black_to_white = random_bool(generator) == 1;
+
+ if (point.blob_index() != blob_index) {
+ point.set_blob_index(blob_index);
+ }
+
+ if (point.theta() != theta) {
+ point.set_theta(theta);
+ }
+
+ if (point.base_x() != x || point.base_y() != y) {
+ point.set_base_xy(x, y);
+ }
+
+ switch (dxy) {
+ case 0:
+ if (point.dx() != 1 || point.dy() != 0) {
+ point.set_dxy(dxy);
+ }
+ break;
+ case 1:
+ if (point.dx() != 1 || point.dy() != 1) {
+ point.set_dxy(dxy);
+ }
+ break;
+ case 2:
+ if (point.dx() != 0 || point.dy() != 1) {
+ point.set_dxy(dxy);
+ }
+ break;
+ case 3:
+ if (point.dx() != -1 || point.dy() != 1) {
+ point.set_dxy(dxy);
+ }
+ break;
+ }
+
+ if (black_to_white != point.black_to_white()) {
+ point.set_black_to_white(black_to_white);
+ }
+
+ EXPECT_EQ(point.blob_index(), blob_index);
+ EXPECT_EQ(point.theta(), theta);
+ EXPECT_EQ(point.base_x(), x);
+ EXPECT_EQ(point.base_y(), y);
+
+ switch (dxy) {
+ case 0:
+ EXPECT_EQ(point.dx(), 1);
+ EXPECT_EQ(point.dy(), 0);
+ break;
+ case 1:
+ EXPECT_EQ(point.dx(), 1);
+ EXPECT_EQ(point.dy(), 1);
+ break;
+ case 2:
+ EXPECT_EQ(point.dx(), 0);
+ EXPECT_EQ(point.dy(), 1);
+ break;
+ case 3:
+ EXPECT_EQ(point.dx(), -1);
+ EXPECT_EQ(point.dy(), 1);
+ break;
+ }
+ EXPECT_EQ(point.x(), x * 2 + point.dx());
+ EXPECT_EQ(point.y(), y * 2 + point.dy());
+
+ EXPECT_EQ(point.black_to_white(), black_to_white);
+ }
+ }
+ }
+ }
+ }
+}
+} // namespace frc971::apriltag::testing
diff --git a/frc971/orin/threshold.cc b/frc971/orin/threshold.cc
new file mode 100644
index 0000000..79ccced
--- /dev/null
+++ b/frc971/orin/threshold.cc
@@ -0,0 +1,206 @@
+#include "frc971/orin/threshold.h"
+
+#include <stdint.h>
+
+#include "frc971/orin/cuda.h"
+
+namespace frc971 {
+namespace apriltag {
+namespace {
+
+// 1280 -> 2 * 128 * 5
+// 720 -> 2 * 8 * 5 * 9
+//
+// 1456 -> 2 * 8 * 7 * 13
+// 1088 -> 2 * 32 * 17
+
+// Writes out the grayscale image and decimated image.
+__global__ void InternalCudaToGreyscaleAndDecimateHalide(
+ const uint8_t *color_image, uint8_t *gray_image, uint8_t *decimated_image,
+ size_t width, size_t height) {
+ size_t i = blockIdx.x * blockDim.x + threadIdx.x;
+ while (i < width * height) {
+ uint8_t pixel = gray_image[i] = color_image[i * 2];
+
+ const size_t row = i / width;
+ const size_t col = i - width * row;
+
+ // Copy over every other pixel.
+ if (row % 2 == 0 && col % 2 == 0) {
+ size_t decimated_row = row / 2;
+ size_t decimated_col = col / 2;
+ decimated_image[decimated_row * width / 2 + decimated_col] = pixel;
+ }
+ i += blockDim.x * gridDim.x;
+ }
+
+ // TODO(austin): Figure out how to load contiguous memory reasonably
+ // efficiently and max/min over it.
+
+ // TODO(austin): Can we do the threshold here too? That would be less memory
+ // bandwidth consumed...
+}
+
+// Returns the min and max for a row of 4 pixels.
+__forceinline__ __device__ uchar2 minmax(uchar4 row) {
+ uint8_t min_val = std::min(std::min(row.x, row.y), std::min(row.z, row.w));
+ uint8_t max_val = std::max(std::max(row.x, row.y), std::max(row.z, row.w));
+ return make_uchar2(min_val, max_val);
+}
+
+// Returns the min and max for a set of min and maxes.
+__forceinline__ __device__ uchar2 minmax(uchar2 val0, uchar2 val1) {
+ return make_uchar2(std::min(val0.x, val1.x), std::max(val0.y, val1.y));
+}
+
+// Returns the pixel index of a pixel at the provided x and y location.
+__forceinline__ __device__ size_t XYToIndex(size_t width, size_t x, size_t y) {
+ return width * y + x;
+}
+
+// Computes the min and max pixel value for each block of 4 pixels.
+__global__ void InternalBlockMinMax(const uint8_t *decimated_image,
+ uchar2 *unfiltered_minmax_image,
+ size_t width, size_t height) {
+ uchar2 vals[4];
+ const size_t x = blockIdx.x * blockDim.x + threadIdx.x;
+ const size_t y = blockIdx.y * blockDim.y + threadIdx.y;
+
+ if (x >= width || y >= height) {
+ return;
+ }
+
+ for (int i = 0; i < 4; ++i) {
+ const uchar4 decimated_block = *reinterpret_cast<const uchar4 *>(
+ decimated_image + XYToIndex(width * 4, x * 4, y * 4 + i));
+
+ vals[i] = minmax(decimated_block);
+ }
+
+ unfiltered_minmax_image[XYToIndex(width, x, y)] =
+ minmax(minmax(vals[0], vals[1]), minmax(vals[2], vals[3]));
+}
+
+// Filters the min/max for the surrounding block of 9 pixels centered on our
+// location using min/max and writes the result back out.
+__global__ void InternalBlockFilter(const uchar2 *unfiltered_minmax_image,
+ uchar2 *minmax_image, size_t width,
+ size_t height) {
+ uchar2 result = make_uchar2(255, 0);
+
+ const size_t x = blockIdx.x * blockDim.x + threadIdx.x;
+ const size_t y = blockIdx.y * blockDim.y + threadIdx.y;
+
+ if (x >= width || y >= height) {
+ return;
+ }
+
+ // Iterate through the 3x3 set of points centered on the point this image is
+ // responsible for, and compute the overall min/max.
+#pragma unroll
+ for (int i = -1; i <= 1; ++i) {
+#pragma unroll
+ for (int j = -1; j <= 1; ++j) {
+ const ssize_t read_x = x + i;
+ const ssize_t read_y = y + j;
+
+ if (read_x < 0 || read_x >= static_cast<ssize_t>(width)) {
+ continue;
+ }
+ if (read_y < 0 || read_y >= static_cast<ssize_t>(height)) {
+ continue;
+ }
+
+ result = minmax(
+ result, unfiltered_minmax_image[XYToIndex(width, read_x, read_y)]);
+ }
+ }
+
+ minmax_image[XYToIndex(width, x, y)] = result;
+}
+
+// Thresholds the image based on the filtered thresholds.
+__global__ void InternalThreshold(const uint8_t *decimated_image,
+ const uchar2 *minmax_image,
+ uint8_t *thresholded_image, size_t width,
+ size_t height, size_t min_white_black_diff) {
+ size_t i = blockIdx.x * blockDim.x + threadIdx.x;
+ while (i < width * height) {
+ const size_t x = i % width;
+ const size_t y = i / width;
+
+ const uchar2 minmax_val = minmax_image[x / 4 + (y / 4) * width / 4];
+
+ uint8_t result;
+ if (minmax_val.y - minmax_val.x < min_white_black_diff) {
+ result = 127;
+ } else {
+ uint8_t thresh = minmax_val.x + (minmax_val.y - minmax_val.x) / 2;
+ if (decimated_image[i] > thresh) {
+ result = 255;
+ } else {
+ result = 0;
+ }
+ }
+
+ thresholded_image[i] = result;
+ i += blockDim.x * gridDim.x;
+ }
+}
+
+} // namespace
+
+void CudaToGreyscaleAndDecimateHalide(
+ const uint8_t *color_image, uint8_t *gray_image, uint8_t *decimated_image,
+ uint8_t *unfiltered_minmax_image, uint8_t *minmax_image,
+ uint8_t *thresholded_image, size_t width, size_t height,
+ size_t min_white_black_diff, CudaStream *stream) {
+ CHECK((width % 8) == 0);
+ CHECK((height % 8) == 0);
+ constexpr size_t kThreads = 256;
+ {
+ // Step one, convert to gray and decimate.
+ size_t kBlocks = (width * height + kThreads - 1) / kThreads / 4;
+ InternalCudaToGreyscaleAndDecimateHalide<<<kBlocks, kThreads, 0,
+ stream->get()>>>(
+ color_image, gray_image, decimated_image, width, height);
+ MaybeCheckAndSynchronize();
+ }
+
+ size_t decimated_width = width / 2;
+ size_t decimated_height = height / 2;
+
+ {
+ // Step 2, compute a min/max for each block of 4x4 (16) pixels.
+ dim3 threads(16, 16, 1);
+ dim3 blocks((decimated_width / 4 + 15) / 16,
+ (decimated_height / 4 + 15) / 16, 1);
+
+ InternalBlockMinMax<<<blocks, threads, 0, stream->get()>>>(
+ decimated_image, reinterpret_cast<uchar2 *>(unfiltered_minmax_image),
+ decimated_width / 4, decimated_height / 4);
+ MaybeCheckAndSynchronize();
+
+ // Step 3, Blur those min/max's a further +- 1 block in each direction using
+ // min/max.
+ InternalBlockFilter<<<blocks, threads, 0, stream->get()>>>(
+ reinterpret_cast<uchar2 *>(unfiltered_minmax_image),
+ reinterpret_cast<uchar2 *>(minmax_image), decimated_width / 4,
+ decimated_height / 4);
+ MaybeCheckAndSynchronize();
+ }
+
+ {
+ // Now, write out 127 if the min/max are too close to each other, or 0/255
+ // if the pixels are above or below the average of the min/max.
+ size_t kBlocks = (width * height / 4 + kThreads - 1) / kThreads / 4;
+ InternalThreshold<<<kBlocks, kThreads, 0, stream->get()>>>(
+ decimated_image, reinterpret_cast<uchar2 *>(minmax_image),
+ thresholded_image, decimated_width, decimated_height,
+ min_white_black_diff);
+ MaybeCheckAndSynchronize();
+ }
+}
+
+} // namespace apriltag
+} // namespace frc971
diff --git a/frc971/orin/threshold.h b/frc971/orin/threshold.h
new file mode 100644
index 0000000..5cdc3a2
--- /dev/null
+++ b/frc971/orin/threshold.h
@@ -0,0 +1,22 @@
+#ifndef FRC971_ORIN_THRESHOLD_H_
+#define FRC971_ORIN_THRESHOLD_H_
+
+#include <stdint.h>
+
+#include "frc971/orin/cuda.h"
+
+namespace frc971 {
+namespace apriltag {
+
+// Converts to grayscale, decimates, and thresholds an image on the provided
+// stream.
+void CudaToGreyscaleAndDecimateHalide(
+ const uint8_t *color_image, uint8_t *gray_image, uint8_t *decimated_image,
+ uint8_t *unfiltered_minmax_image, uint8_t *minmax_image,
+ uint8_t *thresholded_image, size_t width, size_t height,
+ size_t min_white_black_diff, CudaStream *stream);
+
+} // namespace apriltag
+} // namespace frc971
+
+#endif // FRC971_ORIN_THRESHOLD_H_
diff --git a/frc971/orin/transform_output_iterator.h b/frc971/orin/transform_output_iterator.h
new file mode 100644
index 0000000..051baea
--- /dev/null
+++ b/frc971/orin/transform_output_iterator.h
@@ -0,0 +1,92 @@
+#ifndef FRC971_TRANSFORM_OUTPUT_ITERATOR_
+#define FRC971_TRANSFORM_OUTPUT_ITERATOR_
+
+namespace frc971 {
+namespace apriltag {
+
+// template class that allows conversions at the output of a cub algorithm
+template <typename InputType, typename OutputType, typename ConversionOp,
+ typename OffsetT = ptrdiff_t>
+class TransformOutputIterator {
+ private:
+ // proxy object to be able to convert when assigning value
+ struct Reference {
+ OutputType *ptr;
+ ConversionOp convert_op;
+ __host__ __device__ Reference(OutputType *ptr, ConversionOp convert_op)
+ : ptr(ptr), convert_op(convert_op) {}
+ __host__ __device__ Reference operator=(InputType val) {
+ *ptr = convert_op(val);
+ return *this;
+ }
+ };
+
+ public:
+ // typedefs may not be neeeded for iterator to work but is here to maintain
+ // similarity to cub's CacheModifiedOutputIterator
+ typedef TransformOutputIterator self_type;
+ typedef OffsetT difference_type;
+ typedef void value_type;
+ typedef void *pointer;
+ typedef Reference reference;
+
+ TransformOutputIterator(OutputType *ptr, const ConversionOp convert_op)
+ : convert_op(convert_op), ptr(ptr) {}
+
+ // postfix addition
+ __host__ __device__ __forceinline__ self_type operator++(int) {
+ self_type retval = *this;
+ ptr++;
+ return retval;
+ }
+
+ // prefix addition
+ __host__ __device__ __forceinline__ self_type operator++() {
+ ptr++;
+ return *this;
+ }
+
+ // postfix subtraction
+ __host__ __device__ __forceinline__ self_type operator--(int) {
+ self_type retval = *this;
+ ptr--;
+ return retval;
+ }
+
+ // prefix subtraction
+ __host__ __device__ __forceinline__ self_type operator--() {
+ ptr--;
+ return *this;
+ }
+
+ // indirection
+ __host__ __device__ __forceinline__ reference operator*() const {
+ return Reference(ptr, convert_op);
+ }
+
+ // array index
+ __host__ __device__ __forceinline__ reference operator[](int num) const {
+ return Reference(ptr + num, convert_op);
+ }
+
+ // equal to
+ __host__ __device__ __forceinline__ bool operator==(
+ const TransformOutputIterator &rhs) const {
+ return ptr == rhs.ptr;
+ }
+
+ // not equal to
+ __host__ __device__ __forceinline__ bool operator!=(
+ const TransformOutputIterator &rhs) const {
+ return ptr != rhs.ptr;
+ }
+
+ private:
+ const ConversionOp convert_op;
+ OutputType *ptr;
+};
+
+} // namespace apriltag
+} // namespace frc971
+
+#endif // FRC971_TRANSFORM_OUTPUT_ITERATOR_
diff --git a/frc971/wpilib/BUILD b/frc971/wpilib/BUILD
index 33a3cd1..7ce36e4 100644
--- a/frc971/wpilib/BUILD
+++ b/frc971/wpilib/BUILD
@@ -518,6 +518,23 @@
)
cc_library(
+ name = "can_drivetrain_writer",
+ srcs = [
+ "can_drivetrain_writer.cc",
+ ],
+ hdrs = [
+ "can_drivetrain_writer.h",
+ ],
+ target_compatible_with = ["//tools/platforms/hardware:roborio"],
+ deps = [
+ ":falcon",
+ ":loop_output_handler",
+ "//frc971:can_configuration_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_output_fbs",
+ ],
+)
+
+cc_library(
name = "can_sensor_reader",
srcs = ["can_sensor_reader.cc"],
hdrs = ["can_sensor_reader.h"],
diff --git a/frc971/wpilib/can_drivetrain_writer.cc b/frc971/wpilib/can_drivetrain_writer.cc
new file mode 100644
index 0000000..bdff308
--- /dev/null
+++ b/frc971/wpilib/can_drivetrain_writer.cc
@@ -0,0 +1,66 @@
+#include "frc971/wpilib/can_drivetrain_writer.h"
+
+using frc971::wpilib::CANDrivetrainWriter;
+
+CANDrivetrainWriter::CANDrivetrainWriter(::aos::EventLoop *event_loop)
+ : ::frc971::wpilib::LoopOutputHandler<
+ ::frc971::control_loops::drivetrain::Output>(event_loop,
+ "/drivetrain") {
+ event_loop->SetRuntimeRealtimePriority(kDrivetrainWriterPriority);
+
+ event_loop->OnRun([this]() { WriteConfigs(); });
+}
+
+void CANDrivetrainWriter::set_falcons(
+ std::vector<std::shared_ptr<Falcon>> right_falcons,
+ std::vector<std::shared_ptr<Falcon>> left_falcons) {
+ right_falcons_ = std::move(right_falcons);
+ left_falcons_ = std::move(left_falcons);
+}
+
+void CANDrivetrainWriter::HandleCANConfiguration(
+ const CANConfiguration &configuration) {
+ for (auto falcon : right_falcons_) {
+ falcon->PrintConfigs();
+ }
+
+ for (auto falcon : left_falcons_) {
+ falcon->PrintConfigs();
+ }
+
+ if (configuration.reapply()) {
+ WriteConfigs();
+ }
+}
+
+void CANDrivetrainWriter::WriteConfigs() {
+ for (auto falcon : right_falcons_) {
+ falcon->WriteConfigs();
+ }
+
+ for (auto falcon : left_falcons_) {
+ falcon->WriteConfigs();
+ }
+}
+
+void CANDrivetrainWriter::Write(
+ const ::frc971::control_loops::drivetrain::Output &output) {
+ for (auto falcon : right_falcons_) {
+ falcon->WriteVoltage(output.right_voltage());
+ }
+
+ for (auto falcon : left_falcons_) {
+ falcon->WriteVoltage(output.left_voltage());
+ }
+}
+
+void CANDrivetrainWriter::Stop() {
+ AOS_LOG(WARNING, "Drivetrain CAN output too old.\n");
+ for (auto falcon : right_falcons_) {
+ falcon->WriteVoltage(0);
+ }
+
+ for (auto falcon : left_falcons_) {
+ falcon->WriteVoltage(0);
+ }
+}
diff --git a/frc971/wpilib/can_drivetrain_writer.h b/frc971/wpilib/can_drivetrain_writer.h
new file mode 100644
index 0000000..bd396bf
--- /dev/null
+++ b/frc971/wpilib/can_drivetrain_writer.h
@@ -0,0 +1,34 @@
+#include "frc971/can_configuration_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
+#include "frc971/wpilib/falcon.h"
+#include "frc971/wpilib/loop_output_handler.h"
+
+namespace frc971 {
+namespace wpilib {
+
+class CANDrivetrainWriter : public ::frc971::wpilib::LoopOutputHandler<
+ ::frc971::control_loops::drivetrain::Output> {
+ public:
+ CANDrivetrainWriter(::aos::EventLoop *event_loop);
+
+ void set_falcons(std::vector<std::shared_ptr<Falcon>> right_falcons,
+ std::vector<std::shared_ptr<Falcon>> left_falcons);
+
+ void HandleCANConfiguration(const CANConfiguration &configuration);
+
+ static constexpr int kDrivetrainWriterPriority = 35;
+
+ private:
+ void WriteConfigs();
+
+ void Write(
+ const ::frc971::control_loops::drivetrain::Output &output) override;
+
+ void Stop() override;
+
+ std::vector<std::shared_ptr<Falcon>> right_falcons_;
+ std::vector<std::shared_ptr<Falcon>> left_falcons_;
+};
+
+} // namespace wpilib
+} // namespace frc971
diff --git a/motors/button_board.cc b/motors/button_board.cc
index fb17328..1d20fdb 100644
--- a/motors/button_board.cc
+++ b/motors/button_board.cc
@@ -289,7 +289,11 @@
PORTB_PCR19 = PORT_PCR_DSE | PORT_PCR_MUX(2);
// .1ms filter time.
- PORTA_DFWR = PORTB_DFWR = PORTC_DFWR = PORTD_DFWR = PORTE_DFWR = 6000;
+ PORTA_DFWR = 6000;
+ PORTB_DFWR = 6000;
+ PORTC_DFWR = 6000;
+ PORTD_DFWR = 6000;
+ PORTE_DFWR = 6000;
// Set up the buttons. The LEDs pull them up to 5V, so the Teensy needs to not
// be set to pull up.
diff --git a/motors/motor.cc b/motors/motor.cc
index a343301..ed1296c 100644
--- a/motors/motor.cc
+++ b/motors/motor.cc
@@ -40,8 +40,10 @@
"Need to prescale");
void Motor::Init() {
- pwm_ftm_->CNTIN = encoder_ftm_->CNTIN = 0;
- pwm_ftm_->CNT = encoder_ftm_->CNT = 0;
+ pwm_ftm_->CNTIN = 0;
+ encoder_ftm_->CNTIN = 0;
+ pwm_ftm_->CNT = 0;
+ encoder_ftm_->CNT = 0;
pwm_ftm_->MOD = counts_per_cycle() - 1;
encoder_ftm_->MOD = controls_->mechanical_counts_per_revolution() - 1;
diff --git a/motors/peripheral/adc_dma.cc b/motors/peripheral/adc_dma.cc
index d2dc6c9..15d50eb 100644
--- a/motors/peripheral/adc_dma.cc
+++ b/motors/peripheral/adc_dma.cc
@@ -147,17 +147,16 @@
DMAMUX0.CHCFG[reconfigure_dma_channel(0)] = 0;
DMAMUX0.CHCFG[reconfigure_dma_channel(1)] = 0;
- static constexpr ssize_t kResultABOffset =
- static_cast<ssize_t>(offsetof(KINETIS_ADC_t, RB)) -
- static_cast<ssize_t>(offsetof(KINETIS_ADC_t, RA));
+ static constexpr long kResultABOffset =
+ static_cast<long>(offsetof(KINETIS_ADC_t, RB)) -
+ static_cast<long>(offsetof(KINETIS_ADC_t, RA));
static_assert(kResultABOffset > 0, "Offset is backwards");
- static constexpr ssize_t kResultOffsetBits =
- ConstexprLog2(kResultABOffset * 2);
- static constexpr ssize_t kSC1ABOffset =
- static_cast<ssize_t>(offsetof(KINETIS_ADC_t, SC1B)) -
- static_cast<ssize_t>(offsetof(KINETIS_ADC_t, SC1A));
+ static constexpr long kResultOffsetBits = ConstexprLog2(kResultABOffset * 2);
+ static constexpr long kSC1ABOffset =
+ static_cast<long>(offsetof(KINETIS_ADC_t, SC1B)) -
+ static_cast<long>(offsetof(KINETIS_ADC_t, SC1A));
static_assert(kSC1ABOffset > 0, "Offset is backwards");
- static constexpr ssize_t kSC1OffsetBits = ConstexprLog2(kSC1ABOffset * 2);
+ static constexpr long kSC1OffsetBits = ConstexprLog2(kSC1ABOffset * 2);
for (int adc = 0; adc < 2; ++adc) {
// Make sure we can actually write to all the fields in the DMA channels.
DMA.CDNE = result_dma_channel(adc);
@@ -204,16 +203,16 @@
if (next_result_channel != -1) {
link = M_TCD_ELINK | V_TCD_LINKCH(next_result_channel);
}
- result_dma(adc)->CITER = result_dma(adc)->BITER =
- link | 4 /* 4 minor iterations */;
+ result_dma(adc)->CITER = link | 4 /* 4 minor iterations */;
+ result_dma(adc)->BITER = link | 4 /* 4 minor iterations */;
}
{
uint16_t link = 0;
if (next_reconfigure_channel != -1) {
link = M_TCD_ELINK | V_TCD_LINKCH(next_reconfigure_channel);
}
- reconfigure_dma(adc)->CITER = reconfigure_dma(adc)->BITER =
- link | 4 /* 4 minor iterations */;
+ reconfigure_dma(adc)->CITER = link | 4 /* 4 minor iterations */;
+ reconfigure_dma(adc)->BITER = link | 4 /* 4 minor iterations */;
}
result_dma(adc)->DLASTSGA = -(kNumberAdcSamples * sizeof(uint16_t));
reconfigure_dma(adc)->DLASTSGA = 0;
diff --git a/motors/pistol_grip/controller.cc b/motors/pistol_grip/controller.cc
index 5a0d5a2..a88b9a0 100644
--- a/motors/pistol_grip/controller.cc
+++ b/motors/pistol_grip/controller.cc
@@ -792,7 +792,8 @@
uint32_t motor0_sum = 0, motor1_sum = 0, wheel_sum = 0;
// First clear both encoders.
- MOTOR0_ENCODER_FTM->CNT = MOTOR1_ENCODER_FTM->CNT = 0;
+ MOTOR0_ENCODER_FTM->CNT = 0;
+ MOTOR1_ENCODER_FTM->CNT = 0;
for (int i = 0; i < kNumberSamples; ++i) {
delay(1);
@@ -847,7 +848,10 @@
PORTA_PCR13 = PORT_PCR_DSE | PORT_PCR_MUX(2);
// .1ms filter time.
- PORTA_DFWR = PORTC_DFWR = PORTD_DFWR = PORTE_DFWR = 6000;
+ PORTA_DFWR = 6000;
+ PORTC_DFWR = 6000;
+ PORTD_DFWR = 6000;
+ PORTE_DFWR = 6000;
// BTN0
PORTC_PCR7 = PORT_PCR_PE | PORT_PCR_PS | PORT_PCR_MUX(1);
@@ -946,8 +950,8 @@
PIT_TCTRL3 = PIT_TCTRL_TIE | PIT_TCTRL_TEN;
// Have them both wait for the GTB signal.
- FTM0->CONF = FTM3->CONF =
- FTM_CONF_GTBEEN | FTM_CONF_NUMTOF(kSwitchingDivisor - 1);
+ FTM0->CONF = FTM_CONF_GTBEEN | FTM_CONF_NUMTOF(kSwitchingDivisor - 1);
+ FTM3->CONF = FTM_CONF_GTBEEN | FTM_CONF_NUMTOF(kSwitchingDivisor - 1);
// Make FTM3's period half of what it should be so we can get it a half-cycle
// out of phase.
const uint32_t original_mod = FTM3->MOD;
diff --git a/third_party/apriltag/apriltag_quad_thresh.c b/third_party/apriltag/apriltag_quad_thresh.c
index 09dc202..1ce73ad 100644
--- a/third_party/apriltag/apriltag_quad_thresh.c
+++ b/third_party/apriltag/apriltag_quad_thresh.c
@@ -991,7 +991,7 @@
int y = 0;
uint8_t v;
- for (int x = 1; x < w - 1; x++) {
+ for (int x = 1; x < w; x++) {
v = im->buf[y*s + x];
if (v == 127)
@@ -1010,8 +1010,17 @@
uint8_t v_1_m1 = im->buf[(y - 1)*s + 1];
uint8_t v_m1_0;
uint8_t v = im->buf[y*s];
+ {
+ int x = 0;
+ if (v != 127) {
+ DO_UNIONFIND2(0, -1);
+ if (v == 255) {
+ DO_UNIONFIND2(1, -1);
+ }
+ }
+ }
- for (int x = 1; x < w - 1; x++) {
+ for (int x = 1; x < w; x++) {
v_m1_m1 = v_0_m1;
v_0_m1 = v_1_m1;
v_1_m1 = im->buf[(y - 1)*s + x + 1];
@@ -1034,7 +1043,7 @@
if (x == 1 || !(v_m1_0 == v_m1_m1 || v_0_m1 == v_m1_m1) ) {
DO_UNIONFIND2(-1, -1);
}
- if (!(v_0_m1 == v_1_m1)) {
+ if ((x < w - 1) && !(v_0_m1 == v_1_m1)) {
DO_UNIONFIND2(1, -1);
}
}
@@ -1521,46 +1530,52 @@
// A possible optimization would be to combine entries
// within the same cluster.
-#define DO_CONN(dx, dy) \
- if (1) { \
- uint8_t v1 = threshim->buf[(y + dy)*ts + x + dx]; \
- \
- if (v0 + v1 == 255) { \
- uint64_t rep1 = unionfind_get_representative(uf, (y + dy)*w + x + dx); \
- if (unionfind_get_set_size(uf, rep1) > 24) { \
- uint64_t clusterid; \
- if (rep0 < rep1) \
- clusterid = (rep1 << 32) + rep0; \
- else \
- clusterid = (rep0 << 32) + rep1; \
- \
- /* XXX lousy hash function */ \
- uint32_t clustermap_bucket = u64hash_2(clusterid) % nclustermap; \
- struct uint64_zarray_entry *entry = clustermap[clustermap_bucket]; \
- while (entry && entry->id != clusterid) { \
- entry = entry->next; \
- } \
- \
- if (!entry) { \
- if (mem_pool_loc == mem_chunk_size) { \
- mem_pool_loc = 0; \
- mem_pool_idx++; \
- mem_pools[mem_pool_idx] = calloc(mem_chunk_size, sizeof(struct uint64_zarray_entry)); \
- } \
- entry = mem_pools[mem_pool_idx] + mem_pool_loc; \
- mem_pool_loc++; \
- \
- entry->id = clusterid; \
- entry->cluster = zarray_create(sizeof(struct pt)); \
- entry->next = clustermap[clustermap_bucket]; \
- clustermap[clustermap_bucket] = entry; \
- } \
- \
- struct pt p = { .x = 2*x + dx, .y = 2*y + dy, .gx = dx*((int) v1-v0), .gy = dy*((int) v1-v0)}; \
- zarray_add(entry->cluster, &p); \
- } \
- } \
- }
+#define DO_CONN(dx, dy) \
+ if (1) { \
+ uint8_t v1 = threshim->buf[(y + dy) * ts + x + dx]; \
+ \
+ if (v0 + v1 == 255) { \
+ uint64_t rep1 = unionfind_get_representative(uf, (y + dy) * w + x + dx); \
+ if (unionfind_get_set_size(uf, rep1) > 24) { \
+ uint64_t clusterid; \
+ if (rep0 < rep1) \
+ clusterid = (rep1 << 32) + rep0; \
+ else \
+ clusterid = (rep0 << 32) + rep1; \
+ \
+ /* XXX lousy hash function */ \
+ uint32_t clustermap_bucket = u64hash_2(clusterid) % nclustermap; \
+ struct uint64_zarray_entry *entry = clustermap[clustermap_bucket]; \
+ while (entry && entry->id != clusterid) { \
+ entry = entry->next; \
+ } \
+ \
+ if (!entry) { \
+ if (mem_pool_loc == mem_chunk_size) { \
+ mem_pool_loc = 0; \
+ mem_pool_idx++; \
+ mem_pools[mem_pool_idx] = \
+ calloc(mem_chunk_size, sizeof(struct uint64_zarray_entry)); \
+ } \
+ entry = mem_pools[mem_pool_idx] + mem_pool_loc; \
+ mem_pool_loc++; \
+ \
+ entry->id = clusterid; \
+ entry->cluster = zarray_create(sizeof(struct pt)); \
+ entry->next = clustermap[clustermap_bucket]; \
+ clustermap[clustermap_bucket] = entry; \
+ } \
+ \
+ struct pt p = {.x = 2 * x + dx, \
+ .y = 2 * y + dy, \
+ .gx = dx * ((int)v1 - v0), \
+ .gy = dy * ((int)v1 - v0)}; \
+ /*fprintf(stderr, "Adding point %d+%d(%llx) -> (%d, %d)\n", \
+ min(rep0, rep1), max(rep0, rep1), entry->id, p.x, p.y); */ \
+ zarray_add(entry->cluster, &p); \
+ } \
+ } \
+ }
// do 4 connectivity. NB: Arguments must be [-1, 1] or we'll overflow .gx, .gy
DO_CONN(1, 0);
@@ -1796,6 +1811,7 @@
// make segmentation image.
if (td->debug) {
+ srandom(0);
image_u8x3_t *d = image_u8x3_create(w, h);
uint32_t *colors = (uint32_t*) calloc(w*h, sizeof(*colors));
@@ -1804,9 +1820,6 @@
for (int x = 0; x < w; x++) {
uint32_t v = unionfind_get_representative(uf, y*w+x);
- if (unionfind_get_set_size(uf, v) < td->qtp.min_cluster_pixels)
- continue;
-
uint32_t color = colors[v];
uint8_t r = color >> 16,
g = color >> 8,
@@ -1820,6 +1833,10 @@
colors[v] = (r << 16) | (g << 8) | b;
}
+ if (unionfind_get_set_size(uf, v) < 1) //td->qtp.min_cluster_pixels)
+ continue;
+
+
d->buf[y*d->stride + 3*x + 0] = r;
d->buf[y*d->stride + 3*x + 1] = g;
d->buf[y*d->stride + 3*x + 2] = b;
diff --git a/third_party/apriltag/common/timeprofile.h b/third_party/apriltag/common/timeprofile.h
index 8016386..40ab9e6 100644
--- a/third_party/apriltag/common/timeprofile.h
+++ b/third_party/apriltag/common/timeprofile.h
@@ -97,7 +97,7 @@
double parttime = (stamp->utime - lastutime)/1000000.0;
- printf("%2d %32s %15f ms %15f ms\n", i, stamp->name, parttime*1000, cumtime*1000);
+ fprintf(stderr, "%2d %32s %15f ms %15f ms\n", i, stamp->name, parttime*1000, cumtime*1000);
lastutime = stamp->utime;
}
diff --git a/third_party/bazel-toolchain/bazel_tools_changes/tools/cpp/unix_cc_toolchain_config.bzl b/third_party/bazel-toolchain/bazel_tools_changes/tools/cpp/unix_cc_toolchain_config.bzl
index b80b5ec..7b6265f 100755
--- a/third_party/bazel-toolchain/bazel_tools_changes/tools/cpp/unix_cc_toolchain_config.bzl
+++ b/third_party/bazel-toolchain/bazel_tools_changes/tools/cpp/unix_cc_toolchain_config.bzl
@@ -1141,16 +1141,13 @@
)
cuda_flags = [
- "--cuda-gpu-arch=sm_75",
- "--cuda-gpu-arch=sm_80",
- "--cuda-gpu-arch=sm_86",
- "--cuda-gpu-arch=sm_87",
"-x",
"cuda",
]
if ctx.attr.cpu == "aarch64":
cuda_flags += [
+ "--cuda-gpu-arch=sm_87",
"--cuda-path=external/arm64_debian_sysroot/usr/local/cuda-11.8/",
"--ptxas-path=external/arm64_debian_sysroot/usr/local/cuda-11.8/bin/ptxas",
"-D__CUDACC_VER_MAJOR__=11",
@@ -1159,6 +1156,8 @@
pass
elif ctx.attr.cpu == "k8":
cuda_flags += [
+ "--cuda-gpu-arch=sm_75",
+ "--cuda-gpu-arch=sm_86",
"--cuda-path=external/amd64_debian_sysroot/usr/lib/cuda/",
"--ptxas-path=external/amd64_debian_sysroot/usr/bin/ptxas",
"-D__CUDACC_VER_MAJOR__=11",
diff --git a/third_party/flatbuffers/build_defs.bzl b/third_party/flatbuffers/build_defs.bzl
index c1e568f..5f6d71b 100644
--- a/third_party/flatbuffers/build_defs.bzl
+++ b/third_party/flatbuffers/build_defs.bzl
@@ -21,6 +21,7 @@
"--gen-object-api",
"--gen-compare",
"--keep-prefix",
+ "--bfbs-builtins",
"--cpp-std",
"c++17",
"--require-explicit-ids",
diff --git a/tools/cpp/cortex_m4f_crosstool.pb b/tools/cpp/cortex_m4f_crosstool.pb
deleted file mode 100644
index 68114e6..0000000
--- a/tools/cpp/cortex_m4f_crosstool.pb
+++ /dev/null
@@ -1,240 +0,0 @@
-toolchain {
- abi_version: "%NAME%"
- abi_libc_version: "%NAME%"
- builtin_sysroot: ""
- compiler: "gcc"
- host_system_name: "local"
- needsPic: false
- supports_gold_linker: false
- supports_incremental_linker: false
- supports_fission: false
- supports_interface_shared_objects: false
- supports_normalizing_ar: false
- supports_start_end_lib: false
- supports_thin_archives: false
- target_libc: "%NAME%"
- target_cpu: "%NAME%"
- target_system_name: "%NAME%"
- toolchain_identifier: "%NAME%"
-
- tool_path { name: "ar" path: "gcc_arm_none_eabi/arm-none-eabi-ar" }
- tool_path { name: "compat-ld" path: "gcc_arm_none_eabi/arm-none-eabi-ld" }
- tool_path { name: "cpp" path: "gcc_arm_none_eabi/arm-none-eabi-cpp" }
- tool_path { name: "dwp" path: "gcc_arm_none_eabi/arm-none-eabi-dwp" }
- tool_path { name: "gcc" path: "gcc_arm_none_eabi/arm-none-eabi-gcc" }
- tool_path { name: "gcov" path: "gcc_arm_none_eabi/arm-none-eabi-gcov" }
- # C(++) compiles invoke the compiler (as that is the one knowing where
- # to find libraries), but we provide LD so other rules can invoke the linker.
- tool_path { name: "ld" path: "gcc_arm_none_eabi/arm-none-eabi-ld" }
- tool_path { name: "nm" path: "gcc_arm_none_eabi/arm-none-eabi-nm" }
- tool_path { name: "objcopy" path: "gcc_arm_none_eabi/arm-none-eabi-objcopy" }
- objcopy_embed_flag: "-I"
- objcopy_embed_flag: "binary"
- tool_path { name: "objdump" path: "gcc_arm_none_eabi/arm-none-eabi-objdump" }
- tool_path { name: "strip" path: "gcc_arm_none_eabi/arm-none-eabi-strip" }
- linking_mode_flags { mode: FULLY_STATIC }
-
- # TODO(bazel-team): In theory, the path here ought to exactly match the path
- # used by gcc. That works because bazel currently doesn't track files at
- # absolute locations and has no remote execution, yet. However, this will need
- # to be fixed, maybe with auto-detection?
- cxx_builtin_include_directory: '/usr/lib/gcc/arm-none-eabi/4.8/include'
- cxx_builtin_include_directory: '/usr/lib/gcc/arm-none-eabi/4.8/include-fixed'
- cxx_builtin_include_directory: '/usr/lib/arm-none-eabi/include'
- cxx_builtin_include_directory: '/usr/include/newlib',
-
- feature {
- name: "dbg"
- implies: "all_modes"
- flag_set {
- action: "preprocess-assemble"
- action: "c-compile"
- action: "c++-compile"
- action: "c++-header-parsing"
- action: "c++-header-preprocessing"
- action: "c++-module-compile"
- flag_group {
- flag: "-fno-omit-frame-pointer"
- }
- }
- }
-
- feature {
- name: "opt"
- implies: "all_modes"
- }
- feature {
- name: "fastbuild"
- implies: "all_modes"
- }
-
- feature {
- name: "all_modes"
- flag_set {
- action: "preprocess-assemble"
- action: "assemble"
- action: "c-compile"
- flag_group {
- flag: "--std=gnu99"
- }
- }
- flag_set {
- action: "c++-compile"
- action: "c++-header-parsing"
- action: "c++-header-preprocessing"
- action: "c++-module-compile"
- flag_group {
- flag: "--std=gnu++1z"
- flag: "-fno-exceptions"
- flag: "-fno-rtti"
- }
- }
- }
-
- # Anticipated future default.
- # This makes GCC and Clang do what we want when called through symlinks.
- unfiltered_cxx_flag: "-no-canonical-prefixes"
- linker_flag: "-no-canonical-prefixes"
-
- # Things that the code wants defined.
- compiler_flag: "-D__STDC_FORMAT_MACROS"
- compiler_flag: "-D__STDC_CONSTANT_MACROS"
- compiler_flag: "-D__STDC_LIMIT_MACROS"
-
- # Some identifiers for what MCU we're using.
- compiler_flag: "-D%CPU%"
- compiler_flag: "-DF_CPU=%F_CPU%"
-
- compiler_flag: "-Wl,--gc-sections"
-
- # Newlib's stdint.h does this, but GCC's freestanding stdint.h doesn't use
- # newlib's so we have to do it manually...
- compiler_flag: "-D__have_long32"
-
- # Make C++ compilation deterministic. Use linkstamping instead of these
- # compiler symbols.
- unfiltered_cxx_flag: "-Wno-builtin-macro-redefined"
- unfiltered_cxx_flag: "-D__DATE__=\"redacted\""
- unfiltered_cxx_flag: "-D__TIMESTAMP__=\"redacted\""
- unfiltered_cxx_flag: "-D__TIME__=\"redacted\""
-
- # Security hardening on by default.
- # Conservative choice; -D_FORTIFY_SOURCE=2 may be unsafe in some cases.
- # We need to undef it before redefining it as some distributions now have
- # it enabled by default.
- compiler_flag: "-fstack-protector"
- compiler_flag: "-mcpu=cortex-m4"
- compiler_flag: "-mfpu=fpv4-sp-d16"
- compiler_flag: "-mthumb"
- compiler_flag: "-mfloat-abi=hard"
- compiler_flag: "-fno-strict-aliasing"
- linker_flag: "-mcpu=cortex-m4"
- linker_flag: "-mfpu=fpv4-sp-d16"
- linker_flag: "-mthumb"
- linker_flag: "-mfloat-abi=hard"
- linker_flag: "-fno-strict-aliasing"
- linker_flag: "--specs=nano.specs"
-
- # Pretty much everything needs this, including parts of the glibc STL...
- linker_flag: "-lgcc"
- linker_flag: "-lstdc++_nano"
- linker_flag: "-lm"
- linker_flag: "-lc_nano"
- linker_flag: "-T%LINKER_SCRIPT%"
- linker_flag: "-Tmotors/core/kinetis_sections.ld"
-
- compiler_flag: "-fmessage-length=80"
- compiler_flag: "-fmax-errors=20"
-
- compiler_flag: "-Wall"
- compiler_flag: "-Wextra"
- compiler_flag: "-Wpointer-arith"
- compiler_flag: "-Wcast-qual"
- compiler_flag: "-Wwrite-strings"
- compiler_flag: "-Wtype-limits"
- compiler_flag: "-Wsign-compare"
- compiler_flag: "-Wformat=2"
- compiler_flag: "-Werror"
- compiler_flag: "-Wstrict-aliasing=2"
-
- # TODO(Brian): Drop these once we upgrade Eigen.
- compiler_flag: "-Wno-misleading-indentation"
- compiler_flag: "-Wno-int-in-bool-context"
-
- # Be annoying about using doubles places we probably didn't mean to, because
- # the FPU only does single-precision.
- compiler_flag: "-Wdouble-promotion"
-
- # Don't use temp files while compiling.
- compiler_flag: "-pipe"
-
- # Stamp the binary with a unique identifier.
- # TODO(austin): Put these back in.
- #linker_flag: "-Wl,--build-id=md5"
- #linker_flag: "-Wl,--hash-style=gnu"
-
- # Enable debug symbols.
- compiler_flag: "-g"
-
- # Common symbols are weird and not what we want, so just give multiple
- # declaration errors instead.
- compiler_flag: "-fno-common"
-
- # We're not a hosted environment (no file IO, main is called from our code,
- # etc).
- compiler_flag: "-ffreestanding"
- # However, we still want to optimize things like memcpy.
- compiler_flag: "-fbuiltin"
-
- compilation_mode_flags {
- mode: OPT
-
- # Freescale recommends this combination for reducing cycle count.
- # http://www.nxp.com/assets/documents/data/en/application-notes/AN4808.pdf
- compiler_flag: "-O2"
- compiler_flag: "-finline-functions"
-
- # This is definitely worth it for us. It makes the FPU a lot more useful,
- # especially with complex arithmetic, which matters a lot.
- compiler_flag: "-ffast-math"
-
- # It seems like this is a good idea, at least for the number crunching code.
- # Might want to look into moving it to copts on specific rules if the code
- # size increase becomes a problem.
- compiler_flag: "-funroll-loops"
-
- # Disable assertions
- compiler_flag: "-DNDEBUG"
-
- # Removal of unused code and data at link time (can this increase binary size in some cases?).
- compiler_flag: "-ffunction-sections"
- #compiler_flag: "-fdata-sections"
- linker_flag: "-Wl,--gc-sections"
- }
-
- feature {
- name: 'include_paths'
- flag_set {
- action: 'preprocess-assemble'
- action: 'c-compile'
- action: 'c++-compile'
- action: 'c++-header-parsing'
- action: 'c++-header-preprocessing'
- action: 'c++-module-compile'
- flag_group {
- iterate_over: 'quote_include_paths'
- flag: '-iquote'
- flag: '%{quote_include_paths}'
- }
- flag_group {
- iterate_over: 'include_paths'
- flag: '-I%{include_paths}'
- }
- flag_group {
- iterate_over: 'system_include_paths'
- flag: '-I'
- flag: '%{system_include_paths}'
- }
- }
- }
-}
diff --git a/tools/cpp/gcc_arm_none_eabi/arm-none-eabi-gcc b/tools/cpp/gcc_arm_none_eabi/arm-none-eabi-gcc
index 6ff0448..b7146d0 100755
--- a/tools/cpp/gcc_arm_none_eabi/arm-none-eabi-gcc
+++ b/tools/cpp/gcc_arm_none_eabi/arm-none-eabi-gcc
@@ -1,6 +1,6 @@
#!/bin/bash --norc
-PATH="${BAZEL_OUTPUT_ROOT}external/gcc_arm_none_eabi/lib/gcc/arm-none-eabi/7.3.1:$PATH" \
+PATH="${BAZEL_OUTPUT_ROOT}external/gcc_arm_none_eabi/lib/gcc/arm-none-eabi/13.2.1:${BAZEL_OUTPUT_ROOT}external/gcc_arm_none_eabi/libexec/gcc/arm-none-eabi/13.2.1:${PATH}" \
exec \
${BAZEL_OUTPUT_ROOT}external/gcc_arm_none_eabi/bin/arm-none-eabi-gcc \
"$@"
diff --git a/tools/cpp/toolchain_config.bzl b/tools/cpp/toolchain_config.bzl
index 8bab5c7..5e320cd 100644
--- a/tools/cpp/toolchain_config.bzl
+++ b/tools/cpp/toolchain_config.bzl
@@ -296,7 +296,6 @@
"-pipe",
"-g",
"-fno-common",
- "-ffreestanding",
"-fbuiltin",
],
),
@@ -797,7 +796,7 @@
actions = all_cpp_compile_actions,
flag_groups = [
flag_group(
- flags = ["--std=gnu++1z", "-fno-exceptions", "-fno-rtti"],
+ flags = ["--std=gnu++20", "-fno-exceptions", "-fno-rtti"],
),
],
),
diff --git a/tools/dependency_rewrite b/tools/dependency_rewrite
index 2cc90cf..2d2e610 100644
--- a/tools/dependency_rewrite
+++ b/tools/dependency_rewrite
@@ -3,6 +3,7 @@
block *
rewrite maven.ctr-electronics.com/(.*) software.frc971.org/Build-Dependencies/maven.ctr-electronics.com/$1
rewrite github.com/(.*) software.frc971.org/Build-Dependencies/github.com/$1
+rewrite developer.arm.com/(.*) software.frc971.org/Build-Dependencies/developer.arm.com/$1
rewrite dl.google.com/(.*) software.frc971.org/Build-Dependencies/dl.google.com/$1
rewrite mirror.bazel.build/(.*) software.frc971.org/Build-Dependencies/mirror.bazel.build/$1
rewrite nodejs.org/(.*) software.frc971.org/Build-Dependencies/nodejs.org/$1
diff --git a/tools/python/requirements.lock.txt b/tools/python/requirements.lock.txt
index 7470cae..a8a8f4d 100644
--- a/tools/python/requirements.lock.txt
+++ b/tools/python/requirements.lock.txt
@@ -612,6 +612,10 @@
--hash=sha256:47cc05d99aaa09c9e72ed5809b60e7ba354e64b59c9c173ac3018642d8bb41fc \
--hash=sha256:c083dd0dce68dbfbe1129d5271cb90f9447dea7d52097c6e0126120c521ddea8
# via requests
+validators==0.22.0 \
+ --hash=sha256:61cf7d4a62bbae559f2e54aed3b000cea9ff3e2fdbe463f51179b92c58c9585a \
+ --hash=sha256:77b2689b172eeeb600d9605ab86194641670cdb73b60afd577142a9397873370
+ # via -r tools/python/requirements.txt
watchdog==2.1.9 \
--hash=sha256:083171652584e1b8829581f965b9b7723ca5f9a2cd7e20271edf264cfd7c1412 \
--hash=sha256:117ffc6ec261639a0209a3252546b12800670d4bf5f84fbd355957a0595fe654 \
diff --git a/tools/python/requirements.txt b/tools/python/requirements.txt
index 3b3ab3a..0a5f24a 100644
--- a/tools/python/requirements.txt
+++ b/tools/python/requirements.txt
@@ -12,6 +12,7 @@
requests
scipy
shapely
+validators
yapf
# TODO(phil): Migrate to absl-py. These are abandoned as far as I can tell.
diff --git a/tools/python/whl_overrides.json b/tools/python/whl_overrides.json
index 108a544..6cb9998 100644
--- a/tools/python/whl_overrides.json
+++ b/tools/python/whl_overrides.json
@@ -143,6 +143,10 @@
"sha256": "47cc05d99aaa09c9e72ed5809b60e7ba354e64b59c9c173ac3018642d8bb41fc",
"url": "https://software.frc971.org/Build-Dependencies/wheelhouse/urllib3-1.26.13-py2.py3-none-any.whl"
},
+ "validators==0.22.0": {
+ "sha256": "61cf7d4a62bbae559f2e54aed3b000cea9ff3e2fdbe463f51179b92c58c9585a",
+ "url": "https://software.frc971.org/Build-Dependencies/wheelhouse/validators-0.22.0-py3-none-any.whl"
+ },
"watchdog==2.1.9": {
"sha256": "4f4e1c4aa54fb86316a62a87b3378c025e228178d55481d30d857c6c438897d6",
"url": "https://software.frc971.org/Build-Dependencies/wheelhouse/watchdog-2.1.9-py3-none-manylinux2014_x86_64.whl"
diff --git a/tools/rehosting/BUILD b/tools/rehosting/BUILD
new file mode 100644
index 0000000..0eb1a18
--- /dev/null
+++ b/tools/rehosting/BUILD
@@ -0,0 +1,11 @@
+py_test(
+ name = "rehost_test",
+ srcs = ["rehost_test.py"],
+ deps = [":rehost"],
+)
+
+py_binary(
+ name = "rehost",
+ srcs = ["rehost.py"],
+ deps = ["@pip//validators"],
+)
diff --git a/tools/rehosting/README b/tools/rehosting/README
new file mode 100644
index 0000000..42da3bf
--- /dev/null
+++ b/tools/rehosting/README
@@ -0,0 +1,6 @@
+Notes on setup:
+
+1. The buildkite pipeline is set up to enforce that we only run this against
+ the master branch, to prevent users from running un-checked-in scripts to
+ muck with the build dependency server.
+2. Runs against the queue=deploy buildkite queue.
diff --git a/tools/rehosting/rehost.py b/tools/rehosting/rehost.py
new file mode 100644
index 0000000..8e5cd1e
--- /dev/null
+++ b/tools/rehosting/rehost.py
@@ -0,0 +1,61 @@
+from urllib.request import urlopen
+from urllib.parse import urlparse
+import validators
+import shutil
+import stat
+from pathlib import Path
+import os
+import sys
+
+BUILD_DEPENDENCIES_PATH = "/data/files/frc971/Build-Dependencies/"
+WWW_GROUP = "www-data"
+
+
+def get_url() -> str:
+ return sys.argv[1]
+
+
+def validate_url(url: str) -> str:
+ # We have no reason to allow people do download things from IP addresses directly.
+ if not validators.url(
+ url, simple_host=True, skip_ipv4_addr=True, skip_ipv6_addr=True):
+ raise ValueError(f"Invalid URL {url}")
+ return url
+
+
+def url_to_path(url: str) -> Path:
+ parsed = urlparse(url)
+ # Strip out the http:// and any other extraneous junk:
+ path = (Path(BUILD_DEPENDENCIES_PATH) /
+ (parsed.netloc + parsed.path)).resolve()
+ # Confirm that someone didn't sneak in a URL that looks like http://foo.bar/../../../.. or something.
+ path.relative_to(BUILD_DEPENDENCIES_PATH)
+ if path.exists():
+ raise FileExistsError(f"There is already a file uploaded for {url}.")
+ return path
+
+
+def download():
+ url = validate_url(get_url())
+ path = url_to_path(url)
+ path.parent.mkdir(mode=0o775, parents=True, exist_ok=True)
+
+ with urlopen(url) as downloaded:
+ with open(path, 'wb') as output:
+ output.write(downloaded.read())
+
+ relative_path = path.relative_to(BUILD_DEPENDENCIES_PATH)
+ path.chmod(stat.S_IRUSR | stat.S_IRGRP | stat.S_IROTH)
+ try:
+ shutil.chown(path, group=WWW_GROUP)
+ for parent in relative_path.parents:
+ shutil.chown(Path(BUILD_DEPENDENCIES_PATH) / parent,
+ group=WWW_GROUP)
+ except Exception:
+ # The chown's sometimes fail if they get to a manually-created/touched
+ # directory; don't worry about that if it happens..
+ pass
+
+
+if __name__ == "__main__":
+ download()
diff --git a/tools/rehosting/rehost.sh b/tools/rehosting/rehost.sh
new file mode 100755
index 0000000..445d05e
--- /dev/null
+++ b/tools/rehosting/rehost.sh
@@ -0,0 +1,2 @@
+#!/bin/bash
+tools/bazel run //tools/rehosting:rehost -- "$(buildkite-agent meta-data get dependency-url)"
diff --git a/tools/rehosting/rehost_test.py b/tools/rehosting/rehost_test.py
new file mode 100644
index 0000000..da316e2
--- /dev/null
+++ b/tools/rehosting/rehost_test.py
@@ -0,0 +1,37 @@
+import unittest
+import unittest.mock
+from tools.rehosting import rehost
+from pathlib import Path
+import os
+
+
+class TestRehost(unittest.TestCase):
+
+ def test_url_validation(self):
+ self.assertEqual("http://google.com",
+ rehost.validate_url("http://google.com"))
+ self.assertRaisesRegex(Exception, "Invalid URL", rehost.validate_url,
+ "file:///some/secret")
+ self.assertRaisesRegex(Exception, "Invalid URL", rehost.validate_url,
+ "http://10.0.0.0/secret")
+
+ def test_url_to_path(self):
+ test_dir = os.getenv("TEST_TMPDIR", "/tmp/")
+ with unittest.mock.patch.object(rehost, "BUILD_DEPENDENCIES_PATH",
+ test_dir):
+ existing_file = test_dir + "/exists.com"
+ with open(existing_file, 'w') as f:
+ f.write('string')
+ self.assertEqual(
+ Path(test_dir) / "example.com/foo/bar",
+ rehost.url_to_path("https://example.com/foo/bar"))
+ self.assertRaisesRegex(ValueError,
+ f"not in the subpath of '{test_dir}'",
+ rehost.url_to_path,
+ "https://example.com/../../bar")
+ self.assertRaisesRegex(FileExistsError, "There is already a file",
+ rehost.url_to_path, "https://exists.com")
+
+
+if __name__ == "__main__":
+ unittest.main()
diff --git a/y2019/vision/constants.h b/y2019/vision/constants.h
index 8510f4c..959f5b7 100644
--- a/y2019/vision/constants.h
+++ b/y2019/vision/constants.h
@@ -3,6 +3,7 @@
#include <array>
#include <cmath>
+#include <cstdint>
#include <string>
namespace y2019 {
diff --git a/y2023/www/BUILD b/y2023/www/BUILD
index 404247e..94a1a13 100644
--- a/y2023/www/BUILD
+++ b/y2023/www/BUILD
@@ -33,6 +33,8 @@
"//aos/network:web_proxy_ts_fbs",
"//aos/network/www:proxy",
"//frc971/control_loops:control_loops_ts_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_can_position_ts_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_position_ts_fbs",
"//frc971/control_loops/drivetrain:drivetrain_status_ts_fbs",
"//frc971/control_loops/drivetrain/localization:localizer_output_ts_fbs",
"//frc971/vision:target_map_ts_fbs",
diff --git a/y2023/www/field.html b/y2023/www/field.html
index 6bd2fc0..6fe291d 100644
--- a/y2023/www/field.html
+++ b/y2023/www/field.html
@@ -47,6 +47,10 @@
<td>Wrist</td>
<td id="wrist"> NA </td>
</tr>
+ <tr>
+ <td>Wrist Absolute Encoder Value</td>
+ <td id="wrist_encoder"> NA </td>
+ </tr>
</table>
<table>
<tr>
@@ -87,13 +91,37 @@
<td id="arm_roll"> NA </td>
</tr>
<tr>
+ <td>Roll Joint Potentiometer Value</td>
+ <td id="arm_roll_pot"> NA </td>
+ </tr>
+ <tr>
+ <td>Roll Joint Absolute Encoder Value</td>
+ <td id="arm_roll_encoder"> NA </td>
+ </tr>
+ <tr>
<td>Proximal</td>
<td id="arm_proximal"> NA </td>
</tr>
<tr>
+ <td>Proximal Potentiometer Value</td>
+ <td id="arm_proximal_pot"> NA </td>
+ </tr>
+ <tr>
+ <td>Proximal Absolute Encoder Value</td>
+ <td id="arm_proximal_encoder"> NA </td>
+ </tr>
+ <tr>
<td>Distal</td>
<td id="arm_distal"> NA </td>
</tr>
+ <tr>
+ <td>Distal Potentiometer Value</td>
+ <td id="arm_distal_pot"> NA </td>
+ </tr>
+ <tr>
+ <td>Distal Absolute Encoder Value</td>
+ <td id="arm_distal_encoder"> NA </td>
+ </tr>
</table>
<h3>Zeroing Faults:</h3>
@@ -110,6 +138,43 @@
</div>
</div>
</div>
+ <table>
+ <tr>
+ <th colspan="2"> Drivetrain Encoder Positions </th>
+ </tr>
+ <tr>
+ <td> Left Encoder Position</td>
+ <td id="left_drivetrain_encoder"> NA </td>
+ </tr>
+ <tr>
+ <td> Right Encoder Position</td>
+ <td id="right_drivetrain_encoder"> NA </td>
+ </tr>
+ <tr>
+ <td> Right Front Falcon CAN Position</td>
+ <td id="falcon_right_front"> NA </td>
+ </tr>
+ <tr>
+ <td> Right Back Falcon CAN Position</td>
+ <td id="falcon_right_back"> NA </td>
+ </tr>
+ <tr>
+ <td> Right Under Falcon CAN Position</td>
+ <td id="falcon_right_under"> NA </td>
+ </tr>
+ <tr>
+ <td> Left Front Falcon CAN Position</td>
+ <td id="falcon_left_front"> NA </td>
+ </tr>
+ <tr>
+ <td> Left Back Falcon CAN Position</td>
+ <td id="falcon_left_back"> NA </td>
+ </tr>
+ <tr>
+ <td> Left Under Falcon CAN Position</td>
+ <td id="falcon_left_under"> NA </td>
+ </tr>
+ </table>
</body>
</html>
diff --git a/y2023/www/field_handler.ts b/y2023/www/field_handler.ts
index 6952a41..d6ff479 100644
--- a/y2023/www/field_handler.ts
+++ b/y2023/www/field_handler.ts
@@ -3,6 +3,8 @@
import {ServerStatistics, State as ConnectionState} from '../../aos/network/message_bridge_server_generated'
import {Connection} from '../../aos/network/www/proxy'
import {ZeroingError} from '../../frc971/control_loops/control_loops_generated'
+import {Position as DrivetrainPosition} from '../../frc971/control_loops/drivetrain/drivetrain_position_generated'
+import {CANPosition as DrivetrainCANPosition} from '../../frc971/control_loops/drivetrain/drivetrain_can_position_generated'
import {Status as DrivetrainStatus} from '../../frc971/control_loops/drivetrain/drivetrain_status_generated'
import {LocalizerOutput} from '../../frc971/control_loops/drivetrain/localization/localizer_output_generated'
import {TargetMap} from '../../frc971/vision/target_map_generated'
@@ -27,6 +29,8 @@
private canvas = document.createElement('canvas');
private localizerOutput: LocalizerOutput|null = null;
private drivetrainStatus: DrivetrainStatus|null = null;
+ private drivetrainPosition: DrivetrainPosition|null = null;
+ private drivetrainCANPosition: DrivetrainCANPosition|null = null;
private superstructureStatus: SuperstructureStatus|null = null;
// Image information indexed by timestamp (seconds since the epoch), so that
@@ -52,6 +56,8 @@
(document.getElementById('end_effector_state') as HTMLElement);
private wrist: HTMLElement =
(document.getElementById('wrist') as HTMLElement);
+ private wristAbsoluteEncoder: HTMLElement =
+ (document.getElementById('wrist_encoder') as HTMLElement);
private armState: HTMLElement =
(document.getElementById('arm_state') as HTMLElement);
private gamePiece: HTMLElement =
@@ -68,8 +74,38 @@
(document.getElementById('arm_proximal') as HTMLElement);
private distal: HTMLElement =
(document.getElementById('arm_distal') as HTMLElement);
+ private rollPotentiometer: HTMLElement =
+ (document.getElementById('arm_roll_pot') as HTMLElement);
+ private rollAbsoluteEncoder: HTMLElement =
+ (document.getElementById('arm_roll_encoder') as HTMLElement);
+ private proximalPotentiometer: HTMLElement =
+ (document.getElementById('arm_proximal_pot') as HTMLElement);
+ private proximalAbsoluteEncoder: HTMLElement =
+ (document.getElementById('arm_proximal_encoder') as HTMLElement);
+ private distalPotentiometer: HTMLElement =
+ (document.getElementById('arm_distal_pot') as HTMLElement);
+ private distalAbsoluteEncoder: HTMLElement =
+ (document.getElementById('arm_distal_encoder') as HTMLElement);
private zeroingFaults: HTMLElement =
(document.getElementById('zeroing_faults') as HTMLElement);
+
+ private leftDrivetrainEncoder: HTMLElement =
+ (document.getElementById('left_drivetrain_encoder') as HTMLElement);
+ private rightDrivetrainEncoder: HTMLElement =
+ (document.getElementById('right_drivetrain_encoder') as HTMLElement);
+ private falconRightFrontPosition: HTMLElement =
+ (document.getElementById('falcon_right_front') as HTMLElement);
+ private falconRightBackPosition: HTMLElement =
+ (document.getElementById('falcon_right_back') as HTMLElement);
+ private falconRightUnderPosition: HTMLElement =
+ (document.getElementById('falcon_right_under') as HTMLElement);
+ private falconLeftFrontPosition: HTMLElement =
+ (document.getElementById('falcon_left_front') as HTMLElement);
+ private falconLeftBackPosition: HTMLElement =
+ (document.getElementById('falcon_left_back') as HTMLElement);
+ private falconLeftUnderPosition: HTMLElement =
+ (document.getElementById('falcon_left_under') as HTMLElement);
+
constructor(private readonly connection: Connection) {
(document.getElementById('field') as HTMLElement).appendChild(this.canvas);
@@ -155,6 +191,14 @@
this.handleDrivetrainStatus(data);
});
this.connection.addHandler(
+ '/drivetrain', 'frc971.control_loops.drivetrain.Position', (data) => {
+ this.handleDrivetrainPosition(data);
+ });
+ this.connection.addHandler(
+ '/drivetrain', 'y2023.control_loops.drivetrain.CANPosition', (data) => {
+ this.handleDrivetrainCANPosition(data);
+ });
+ this.connection.addHandler(
'/localizer', 'frc971.controls.LocalizerOutput', (data) => {
this.handleLocalizerOutput(data);
});
@@ -210,6 +254,16 @@
this.drivetrainStatus = DrivetrainStatus.getRootAsStatus(fbBuffer);
}
+ private handleDrivetrainPosition(data: Uint8Array): void {
+ const fbBuffer = new ByteBuffer(data);
+ this.drivetrainPosition = DrivetrainPosition.getRootAsPosition(fbBuffer);
+ }
+
+ private handleDrivetrainCANPosition(data: Uint8Array): void {
+ const fbBuffer = new ByteBuffer(data);
+ this.drivetrainCANPosition = DrivetrainCANPosition.getRootAsCANPosition(fbBuffer);
+ }
+
private handleSuperstructureStatus(data: Uint8Array): void {
const fbBuffer = new ByteBuffer(data);
this.superstructureStatus = SuperstructureStatus.getRootAsStatus(fbBuffer);
@@ -386,6 +440,8 @@
this.superstructureStatus.wrist().estimatorState().position(),
1e-3);
}
+ this.wristAbsoluteEncoder.innerHTML = this.superstructureStatus.wrist()
+ .estimatorState().position().toString();
this.armState.innerHTML =
ArmState[this.superstructureStatus.arm().state()];
this.gamePiece.innerHTML = Class[this.superstructureStatus.gamePiece()];
@@ -399,6 +455,14 @@
.rollJointEstimatorState()
.position()
.toFixed(2);
+ this.rollPotentiometer.innerHTML = this.superstructureStatus.arm()
+ .rollJointEstimatorState()
+ .potPosition()
+ .toString();
+ this.rollAbsoluteEncoder.innerHTML = this.superstructureStatus.arm()
+ .rollJointEstimatorState()
+ .absolutePosition()
+ .toString();
this.proximal.innerHTML = this.superstructureStatus.arm()
.proximalEstimatorState()
.position()
@@ -407,6 +471,22 @@
.distalEstimatorState()
.position()
.toFixed(2);
+ this.proximalPotentiometer.innerHTML = this.superstructureStatus.arm()
+ .proximalEstimatorState()
+ .potPosition()
+ .toString();
+ this.proximalAbsoluteEncoder.innerHTML = this.superstructureStatus.arm()
+ .proximalEstimatorState()
+ .absolutePosition()
+ .toString();
+ this.distalPotentiometer.innerHTML = this.superstructureStatus.arm()
+ .distalEstimatorState()
+ .potPosition()
+ .toString();
+ this.distalAbsoluteEncoder.innerHTML = this.superstructureStatus.arm()
+ .distalEstimatorState()
+ .absolutePosition()
+ .toString();
let zeroingErrors: string = 'Roll Joint Errors:' +
'<br/>';
for (let i = 0; i < this.superstructureStatus.arm()
@@ -463,6 +543,34 @@
false);
}
+ if (this.drivetrainPosition) {
+ this.leftDrivetrainEncoder.innerHTML =
+ this.drivetrainPosition.leftEncoder().toString();
+
+ this.rightDrivetrainEncoder.innerHTML =
+ this.drivetrainPosition.rightEncoder().toString();
+ }
+
+ if (this.drivetrainCANPosition) {
+ this.falconRightFrontPosition.innerHTML = //TODO: (niko) Improve this so that falcons are not hard-coded
+ this.drivetrainCANPosition.falcons(0).position().toString();
+
+ this.falconRightBackPosition.innerHTML =
+ this.drivetrainCANPosition.falcons(1).position().toString();
+
+ this.falconRightUnderPosition.innerHTML =
+ this.drivetrainCANPosition.falcons(2).position().toString();
+
+ this.falconLeftFrontPosition.innerHTML =
+ this.drivetrainCANPosition.falcons(3).position().toString();
+
+ this.falconLeftBackPosition.innerHTML =
+ this.drivetrainCANPosition.falcons(4).position().toString();
+
+ this.falconLeftUnderPosition.innerHTML =
+ this.drivetrainCANPosition.falcons(5).position().toString();
+ }
+
if (this.localizerOutput) {
if (!this.localizerOutput.zeroed()) {
this.setZeroing(this.x);
diff --git a/y2023_bot3/BUILD b/y2023_bot3/BUILD
index c9258f0..86625da 100644
--- a/y2023_bot3/BUILD
+++ b/y2023_bot3/BUILD
@@ -158,7 +158,6 @@
"//frc971/shooter_interpolation:interpolation",
"//frc971/zeroing:pot_and_absolute_encoder",
"//y2023_bot3/control_loops/drivetrain:polydrivetrain_plants",
- "//y2023_bot3/control_loops/superstructure/pivot_joint:pivot_joint_plants",
"@com_github_google_glog//:glog",
"@com_google_absl//absl/base",
],
diff --git a/y2023_bot3/autonomous/autonomous_actor.cc b/y2023_bot3/autonomous/autonomous_actor.cc
index fc794b1..7c722be 100644
--- a/y2023_bot3/autonomous/autonomous_actor.cc
+++ b/y2023_bot3/autonomous/autonomous_actor.cc
@@ -161,9 +161,6 @@
<< "Expect at least one JoystickState message before running auto...";
alliance_ = joystick_state_fetcher_->alliance();
- preloaded_ = false;
- roller_goal_ = control_loops::superstructure::RollerGoal::IDLE;
- pivot_goal_ = control_loops::superstructure::PivotGoal::NEUTRAL;
SendSuperstructureGoal();
}
@@ -227,101 +224,25 @@
auto &splines = *charged_up_splines_;
- AOS_LOG(INFO, "Going to preload");
-
- // Tell the superstructure that a cube was preloaded
- if (!WaitForPreloaded()) {
- return;
- }
-
- // Place & Spit firt cube high
- AOS_LOG(INFO, "Moving arm to front high scoring position");
-
- HighScore();
- std::this_thread::sleep_for(chrono::milliseconds(600));
-
- SpitHigh();
- std::this_thread::sleep_for(chrono::milliseconds(600));
-
- StopSpitting();
-
- std::this_thread::sleep_for(chrono::milliseconds(200));
- AOS_LOG(
- INFO, "Placed first cube (HIGH) %lf s\n",
- aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
-
- if (FLAGS_one_piece) {
- return;
- }
-
// Drive to second cube
if (!splines[0].WaitForPlan()) {
return;
}
splines[0].Start();
- // Move arm into position to intake cube and intake.
- AOS_LOG(INFO, "Moving arm to back pickup position");
-
- Pickup();
-
- std::this_thread::sleep_for(chrono::milliseconds(500));
- Intake();
-
- AOS_LOG(
- INFO, "Turning on rollers %lf s\n",
- aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
-
- if (!splines[0].WaitForSplineDistanceRemaining(0.02)) {
- return;
- }
-
- AOS_LOG(
- INFO, "Got there %lf s\n",
- aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
-
- // Drive back to grid
if (!splines[1].WaitForPlan()) {
return;
}
splines[1].Start();
- std::this_thread::sleep_for(chrono::milliseconds(600));
- // Place Low
- AOS_LOG(INFO, "Moving arm to front mid scoring position");
-
- MidScore();
-
- std::this_thread::sleep_for(chrono::milliseconds(600));
if (!splines[1].WaitForSplineDistanceRemaining(0.1)) return;
- Spit();
- std::this_thread::sleep_for(chrono::milliseconds(400));
- StopSpitting();
-
- AOS_LOG(
- INFO, "Placed second cube (MID) %lf s\n",
- aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
-
// Drive to third cube
if (!splines[2].WaitForPlan()) {
return;
}
splines[2].Start();
- std::this_thread::sleep_for(chrono::milliseconds(500));
- // Move arm into position to intake cube and intake.
- AOS_LOG(INFO, "Moving arm to back pickup position");
-
- Pickup();
-
- std::this_thread::sleep_for(chrono::milliseconds(250));
- Intake();
-
- AOS_LOG(
- INFO, "Turning on rollers %lf s\n",
- aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
-
if (!splines[2].WaitForSplineDistanceRemaining(0.02)) {
return;
}
@@ -337,56 +258,17 @@
splines[3].Start();
std::this_thread::sleep_for(chrono::milliseconds(600));
- // Place Low
- AOS_LOG(INFO, "Moving arm to front low scoring position");
-
- LowScore();
-
- std::this_thread::sleep_for(chrono::milliseconds(600));
if (!splines[3].WaitForSplineDistanceRemaining(0.1)) return;
-
- Spit();
- std::this_thread::sleep_for(chrono::milliseconds(600));
- StopSpitting();
-
- AOS_LOG(
- INFO, "Placed low cube (LOW) %lf s\n",
- aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
}
// Charged Up Place and Mobility Autonomous (middle)
void AutonomousActor::ChargedUpMiddle() {
- aos::monotonic_clock::time_point start_time = aos::monotonic_clock::now();
-
CHECK(charged_up_middle_splines_);
auto &splines = *charged_up_middle_splines_;
AOS_LOG(INFO, "Going to preload");
- // Tell the superstructure that a cube was preloaded
- if (!WaitForPreloaded()) {
- return;
- }
-
- // Place & Spit firt cube mid
- AOS_LOG(INFO, "Moving arm to front mid scoring position");
-
- MidScore();
- std::this_thread::sleep_for(chrono::milliseconds(300));
-
- Spit();
- std::this_thread::sleep_for(chrono::milliseconds(300));
-
- StopSpitting();
-
- std::this_thread::sleep_for(chrono::milliseconds(100));
- AOS_LOG(
- INFO, "Placed first cube (Mid) %lf s\n",
- aos::time::DurationInSeconds(aos::monotonic_clock::now() - start_time));
-
- // Drive to second cube
-
if (!splines[0].WaitForPlan()) {
return;
}
@@ -399,83 +281,11 @@
control_loops::superstructure::Goal::Builder superstructure_builder =
builder.MakeBuilder<control_loops::superstructure::Goal>();
- superstructure_builder.add_pivot_goal(pivot_goal_);
- superstructure_builder.add_roller_goal(roller_goal_);
- superstructure_builder.add_preloaded_with_cube(preloaded_);
-
if (builder.Send(superstructure_builder.Finish()) !=
aos::RawSender::Error::kOk) {
AOS_LOG(ERROR, "Sending superstructure goal failed.\n");
}
}
-[[nodiscard]] bool AutonomousActor::WaitForPreloaded() {
- set_preloaded(true);
- SendSuperstructureGoal();
-
- ::aos::time::PhasedLoop phased_loop(frc971::controls::kLoopFrequency,
- event_loop()->monotonic_now(),
- ActorBase::kLoopOffset);
-
- bool loaded = false;
- while (!loaded) {
- if (ShouldCancel()) {
- return false;
- }
-
- phased_loop.SleepUntilNext();
- superstructure_status_fetcher_.Fetch();
- CHECK(superstructure_status_fetcher_.get() != nullptr);
-
- loaded = (superstructure_status_fetcher_->end_effector_state() ==
- control_loops::superstructure::EndEffectorState::LOADED);
- }
-
- set_preloaded(false);
- SendSuperstructureGoal();
-
- return true;
-}
-
-void AutonomousActor::HighScore() {
- set_pivot_goal(control_loops::superstructure::PivotGoal::SCORE_HIGH_FRONT);
- SendSuperstructureGoal();
-}
-void AutonomousActor::MidScore() {
- set_pivot_goal(control_loops::superstructure::PivotGoal::SCORE_MID_FRONT);
- SendSuperstructureGoal();
-}
-void AutonomousActor::LowScore() {
- set_pivot_goal(control_loops::superstructure::PivotGoal::SCORE_LOW_FRONT);
- SendSuperstructureGoal();
-}
-void AutonomousActor::Spit() {
- set_roller_goal(control_loops::superstructure::RollerGoal::SPIT);
- SendSuperstructureGoal();
-}
-void AutonomousActor::SpitHigh() {
- set_roller_goal(control_loops::superstructure::RollerGoal::SPIT_HIGH);
- SendSuperstructureGoal();
-}
-
-void AutonomousActor::StopSpitting() {
- set_roller_goal(control_loops::superstructure::RollerGoal::IDLE);
- SendSuperstructureGoal();
-}
-void AutonomousActor::Intake() {
- set_roller_goal(control_loops::superstructure::RollerGoal::INTAKE_CUBE);
- SendSuperstructureGoal();
-}
-
-void AutonomousActor::Pickup() {
- set_pivot_goal(control_loops::superstructure::PivotGoal::PICKUP_BACK);
- SendSuperstructureGoal();
-}
-
-void AutonomousActor::Neutral() {
- set_pivot_goal(control_loops::superstructure::PivotGoal::NEUTRAL);
- SendSuperstructureGoal();
-}
-
} // namespace autonomous
} // namespace y2023_bot3
diff --git a/y2023_bot3/autonomous/autonomous_actor.h b/y2023_bot3/autonomous/autonomous_actor.h
index ad33eed..bc0a35c 100644
--- a/y2023_bot3/autonomous/autonomous_actor.h
+++ b/y2023_bot3/autonomous/autonomous_actor.h
@@ -11,8 +11,6 @@
#include "y2023_bot3/control_loops/superstructure/superstructure_goal_generated.h"
#include "y2023_bot3/control_loops/superstructure/superstructure_status_generated.h"
-// TODO<FILIP>: Add NEUTRAL pivot pose.
-
namespace y2023_bot3 {
namespace autonomous {
@@ -24,32 +22,8 @@
const ::frc971::autonomous::AutonomousActionParams *params) override;
private:
- void set_preloaded(bool preloaded) { preloaded_ = preloaded; }
-
- void set_pivot_goal(
- control_loops::superstructure::PivotGoal requested_pivot_goal) {
- pivot_goal_ = requested_pivot_goal;
- }
-
- void set_roller_goal(
- control_loops::superstructure::RollerGoal requested_roller_goal) {
- roller_goal_ = requested_roller_goal;
- }
-
void SendSuperstructureGoal();
- [[nodiscard]] bool WaitForPreloaded();
-
- void HighScore();
- void MidScore();
- void LowScore();
- void Spit();
- void SpitHigh();
- void StopSpitting();
- void Pickup();
- void Intake();
- void Neutral();
-
void Reset();
void SendStartingPosition(const Eigen::Vector3d &start);
@@ -76,11 +50,6 @@
std::optional<Eigen::Vector3d> starting_position_;
- bool preloaded_ = false;
-
- control_loops::superstructure::PivotGoal pivot_goal_;
- control_loops::superstructure::RollerGoal roller_goal_;
-
aos::Sender<control_loops::superstructure::Goal> superstructure_goal_sender_;
aos::Fetcher<y2023_bot3::control_loops::superstructure::Status>
superstructure_status_fetcher_;
diff --git a/y2023_bot3/constants.cc b/y2023_bot3/constants.cc
index 0a99b94..7f77d4d 100644
--- a/y2023_bot3/constants.cc
+++ b/y2023_bot3/constants.cc
@@ -12,7 +12,6 @@
#include "aos/mutex/mutex.h"
#include "aos/network/team_number.h"
-#include "y2023_bot3/control_loops/superstructure/pivot_joint/integral_pivot_joint_plant.h"
namespace y2023_bot3 {
namespace constants {
@@ -21,41 +20,13 @@
LOG(INFO) << "creating a Constants for team: " << team;
Values r;
- auto *const pivot_joint = &r.pivot_joint;
-
- r.pivot_joint_flipped = true;
-
- pivot_joint->subsystem_params.zeroing_voltage = 3.0;
- pivot_joint->subsystem_params.operating_voltage = 12.0;
- pivot_joint->subsystem_params.zeroing_profile_params = {0.5, 3.0};
- pivot_joint->subsystem_params.default_profile_params = {5.0, 5.0};
- pivot_joint->subsystem_params.range = Values::kPivotJointRange();
- pivot_joint->subsystem_params.make_integral_loop =
- control_loops::superstructure::pivot_joint::MakeIntegralPivotJointLoop;
- pivot_joint->subsystem_params.zeroing_constants.average_filter_size =
- Values::kZeroingSampleSize;
- pivot_joint->subsystem_params.zeroing_constants.one_revolution_distance =
- M_PI * 2.0 * constants::Values::kPivotJointEncoderRatio();
- pivot_joint->subsystem_params.zeroing_constants.zeroing_threshold = 0.0005;
- pivot_joint->subsystem_params.zeroing_constants.moving_buffer_size = 20;
- pivot_joint->subsystem_params.zeroing_constants.allowable_encoder_error = 0.9;
switch (team) {
// A set of constants for tests.
case 1:
-
- pivot_joint->potentiometer_offset = 0.0;
-
- pivot_joint->subsystem_params.zeroing_constants
- .measured_absolute_position = 0.0;
-
break;
case kThirdRobotTeamNumber:
- pivot_joint->subsystem_params.zeroing_constants
- .measured_absolute_position = 0.564786179025525;
-
- pivot_joint->potentiometer_offset = 0.304569457401797 + 2.66972311194163;
break;
default:
diff --git a/y2023_bot3/constants.h b/y2023_bot3/constants.h
index ba7267c..60fe404 100644
--- a/y2023_bot3/constants.h
+++ b/y2023_bot3/constants.h
@@ -10,7 +10,6 @@
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
#include "frc971/zeroing/pot_and_absolute_encoder.h"
#include "y2023_bot3/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
-#include "y2023_bot3/control_loops/superstructure/pivot_joint/pivot_joint_plant.h"
namespace y2023_bot3 {
namespace constants {
@@ -40,12 +39,6 @@
static constexpr double kDrivetrainSupplyCurrentLimit() { return 35.0; }
static constexpr double kDrivetrainStatorCurrentLimit() { return 60.0; }
- static constexpr double kRollerSupplyCurrentLimit() { return 30.0; }
- static constexpr double kRollerStatorCurrentLimit() { return 100.0; }
-
- static constexpr double kPivotSupplyCurrentLimit() { return 40.0; }
- static constexpr double kPivotStatorCurrentLimit() { return 200.0; }
-
// timeout to ensure code doesn't get stuck after releasing the "intake"
// button
static constexpr std::chrono::milliseconds kExtraIntakingTime() {
@@ -63,51 +56,6 @@
return (rotations * (2.0 * M_PI)) *
control_loops::drivetrain::kHighOutputRatio;
}
-
- // Pivot Joint
- static constexpr double kPivotJointEncoderCountsPerRevolution() {
- return 4096.0;
- }
-
- static constexpr double kPivotJointEncoderRatio() {
- return (24.0 / 64.0) * (15.0 / 60.0);
- }
-
- static constexpr double kPivotJointPotRatio() {
- return (24.0 / 64.0) * (15.0 / 60.0);
- }
-
- static constexpr double kPivotJointPotRadiansPerVolt() {
- return kPivotJointPotRatio() * (10.0 /*turns*/ / 5.0 /*volts*/) *
- (2 * M_PI /*radians*/);
- }
-
- static constexpr double kMaxPivotJointEncoderPulsesPerSecond() {
- return control_loops::superstructure::pivot_joint::kFreeSpeed /
- (2.0 * M_PI) *
- control_loops::superstructure::pivot_joint::kOutputRatio /
- kPivotJointEncoderRatio() * kPivotJointEncoderCountsPerRevolution();
- }
-
- static constexpr ::frc971::constants::Range kPivotJointRange() {
- return ::frc971::constants::Range{
- .lower_hard = -1.78879503977269, // Back Hard
- .upper_hard = 1.76302285774785, // Front Hard
- .lower = -1.77156498873494, // Back Soft
- .upper = 1.76555657862879, // Front Soft
- };
- }
-
- struct PotAndAbsEncoderConstants {
- ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemParams<
- ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator>
- subsystem_params;
- double potentiometer_offset;
- };
-
- PotAndAbsEncoderConstants pivot_joint;
-
- bool pivot_joint_flipped;
};
// Creates and returns a Values instance for the constants.
diff --git a/y2023_bot3/control_loops/python/BUILD b/y2023_bot3/control_loops/python/BUILD
index 2753020..fe7666f 100644
--- a/y2023_bot3/control_loops/python/BUILD
+++ b/y2023_bot3/control_loops/python/BUILD
@@ -55,19 +55,3 @@
visibility = ["//visibility:public"],
deps = ["//y2023_bot3/control_loops:python_init"],
)
-
-py_binary(
- name = "pivot_joint",
- srcs = [
- "pivot_joint.py",
- ],
- legacy_create_init = False,
- target_compatible_with = ["@platforms//cpu:x86_64"],
- deps = [
- ":python_init",
- "//frc971/control_loops/python:angular_system",
- "//frc971/control_loops/python:controls",
- "@pip//glog",
- "@pip//python_gflags",
- ],
-)
diff --git a/y2023_bot3/control_loops/python/pivot_joint.py b/y2023_bot3/control_loops/python/pivot_joint.py
deleted file mode 100644
index baea920..0000000
--- a/y2023_bot3/control_loops/python/pivot_joint.py
+++ /dev/null
@@ -1,60 +0,0 @@
-#!/usr/bin/python3
-
-from aos.util.trapezoid_profile import TrapezoidProfile
-from frc971.control_loops.python import control_loop
-from frc971.control_loops.python import angular_system
-from frc971.control_loops.python import controls
-import numpy
-import sys
-from matplotlib import pylab
-import gflags
-import glog
-
-FLAGS = gflags.FLAGS
-
-try:
- gflags.DEFINE_bool("plot", False, "If true, plot the loop response.")
-except gflags.DuplicateFlagError:
- pass
-
-kPivotJoint = angular_system.AngularSystemParams(
- name="PivotJoint",
- motor=control_loop.Falcon(),
- G=(14 / 50) * (24 / 64) * (24 / 64) * (15 / 60),
- # Use parallel axis theorem to get the moment of inertia around
- # the joint (I = I_cm + mh^2 = 0.001877 + 0.8332 * 0.0407162^2)
- J=(0.13372 * 2.00),
- q_pos=0.80,
- q_vel=80.0,
- kalman_q_pos=0.12,
- kalman_q_vel=2.0,
- kalman_q_voltage=1.5,
- kalman_r_position=0.05,
- radius=5.71 * 0.0254,
-)
-
-
-def main(argv):
- if FLAGS.plot:
- R = numpy.matrix([[numpy.pi / 2.0], [0.0]])
- angular_system.PlotKick(kPivotJoint, R)
- angular_system.PlotMotion(kPivotJoint, R)
- return
-
- # Write the generated constants out to a file.
- if len(argv) != 5:
- glog.fatal(
- "Expected .h file name and .cc file name for the pivot joint and integral pivot joint."
- )
- else:
- namespaces = [
- "y2023_bot3", "control_loops", "superstructure", "pivot_joint"
- ]
- angular_system.WriteAngularSystem(kPivotJoint, argv[1:3], argv[3:5],
- namespaces)
-
-
-if __name__ == "__main__":
- argv = FLAGS(sys.argv)
- glog.init()
- sys.exit(main(argv))
diff --git a/y2023_bot3/control_loops/superstructure/BUILD b/y2023_bot3/control_loops/superstructure/BUILD
index 8630ef9..fe41eed 100644
--- a/y2023_bot3/control_loops/superstructure/BUILD
+++ b/y2023_bot3/control_loops/superstructure/BUILD
@@ -61,24 +61,6 @@
)
cc_library(
- name = "end_effector",
- srcs = [
- "end_effector.cc",
- ],
- hdrs = [
- "end_effector.h",
- ],
- deps = [
- ":superstructure_goal_fbs",
- ":superstructure_status_fbs",
- "//aos/events:event_loop",
- "//aos/time",
- "//frc971/control_loops:control_loop",
- "//y2023_bot3:constants",
- ],
-)
-
-cc_library(
name = "superstructure_lib",
srcs = [
"superstructure.cc",
@@ -89,8 +71,6 @@
data = [
],
deps = [
- ":end_effector",
- ":pivot_joint",
":superstructure_goal_fbs",
":superstructure_output_fbs",
":superstructure_position_fbs",
@@ -189,21 +169,3 @@
"//aos/network:team_number",
],
)
-
-cc_library(
- name = "pivot_joint",
- srcs = [
- "pivot_joint.cc",
- ],
- hdrs = [
- "pivot_joint.h",
- ],
- deps = [
- ":superstructure_goal_fbs",
- ":superstructure_status_fbs",
- "//aos/events:event_loop",
- "//aos/time",
- "//frc971/control_loops:control_loop",
- "//y2023_bot3:constants",
- ],
-)
diff --git a/y2023_bot3/control_loops/superstructure/end_effector.cc b/y2023_bot3/control_loops/superstructure/end_effector.cc
deleted file mode 100644
index 665aa9d..0000000
--- a/y2023_bot3/control_loops/superstructure/end_effector.cc
+++ /dev/null
@@ -1,120 +0,0 @@
-#include "y2023_bot3/control_loops/superstructure/end_effector.h"
-
-#include "aos/events/event_loop.h"
-#include "aos/time/time.h"
-#include "frc971/control_loops/control_loop.h"
-
-namespace y2023_bot3 {
-namespace control_loops {
-namespace superstructure {
-
-using ::aos::monotonic_clock;
-
-EndEffector::EndEffector()
- : state_(EndEffectorState::IDLE), timer_(aos::monotonic_clock::min_time) {}
-
-void EndEffector::RunIteration(
- const ::aos::monotonic_clock::time_point timestamp, RollerGoal roller_goal,
- bool beambreak_status, double *roller_voltage, bool preloaded_with_cube) {
- *roller_voltage = 0.0;
-
- // If we started off preloaded, skip to the loaded state.
- // Make sure we weren't already there just in case.
- if (preloaded_with_cube) {
- switch (state_) {
- case EndEffectorState::IDLE:
- case EndEffectorState::INTAKING:
- state_ = EndEffectorState::LOADED;
- break;
- case EndEffectorState::LOADED:
- break;
- case EndEffectorState::SPITTING:
- break;
- case EndEffectorState::SPITTING_MID:
- break;
- case EndEffectorState::SPITTING_HIGH:
- break;
- }
- }
-
- if (roller_goal == RollerGoal::SPIT) {
- state_ = EndEffectorState::SPITTING;
- }
-
- if (roller_goal == RollerGoal::SPIT_MID) {
- state_ = EndEffectorState::SPITTING_MID;
- }
-
- if (roller_goal == RollerGoal::SPIT_HIGH) {
- state_ = EndEffectorState::SPITTING_HIGH;
- }
-
- switch (state_) {
- case EndEffectorState::IDLE:
- // If idle and intake requested, intake
- if (roller_goal == RollerGoal::INTAKE_CUBE) {
- state_ = EndEffectorState::INTAKING;
- timer_ = timestamp;
- }
- break;
- case EndEffectorState::INTAKING:
- // If intaking and beam break is not triggered, keep intaking
- if (roller_goal == RollerGoal::INTAKE_CUBE) {
- timer_ = timestamp;
- }
-
- if (beambreak_status) {
- // Beam has been broken, switch to loaded.
- state_ = EndEffectorState::LOADED;
- break;
- } else if (timestamp > timer_ + constants::Values::kExtraIntakingTime()) {
- // Intaking failed, waited 1 second with no beambreak
- state_ = EndEffectorState::IDLE;
- break;
- }
-
- *roller_voltage = kRollerCubeSuckVoltage();
-
- break;
- case EndEffectorState::LOADED:
- timer_ = timestamp;
- if (!preloaded_with_cube && !beambreak_status) {
- state_ = EndEffectorState::INTAKING;
- break;
- }
-
- break;
-
- case EndEffectorState::SPITTING:
- *roller_voltage = kRollerCubeSpitVoltage();
-
- if (roller_goal == RollerGoal::IDLE) {
- state_ = EndEffectorState::IDLE;
- }
-
- break;
-
- case EndEffectorState::SPITTING_MID:
- *roller_voltage = kRollerCubeSpitMidVoltage();
-
- if (roller_goal == RollerGoal::IDLE) {
- state_ = EndEffectorState::IDLE;
- }
-
- break;
-
- case EndEffectorState::SPITTING_HIGH:
- *roller_voltage = kRollerCubeSpitHighVoltage();
-
- if (roller_goal == RollerGoal::IDLE) {
- state_ = EndEffectorState::IDLE;
- }
- break;
- }
-}
-
-void EndEffector::Reset() { state_ = EndEffectorState::IDLE; }
-
-} // namespace superstructure
-} // namespace control_loops
-} // namespace y2023_bot3
\ No newline at end of file
diff --git a/y2023_bot3/control_loops/superstructure/end_effector.h b/y2023_bot3/control_loops/superstructure/end_effector.h
deleted file mode 100644
index be0abc0..0000000
--- a/y2023_bot3/control_loops/superstructure/end_effector.h
+++ /dev/null
@@ -1,40 +0,0 @@
-#ifndef Y2023_BOT3_CONTROL_LOOPS_SUPERSTRUCTURE_END_EFFECTOR_H_
-#define Y2023_BOT3_CONTROL_LOOPS_SUPERSTRUCTURE_END_EFFECTOR_H_
-
-#include "aos/events/event_loop.h"
-#include "aos/time/time.h"
-#include "frc971/control_loops/control_loop.h"
-#include "y2023_bot3/constants.h"
-#include "y2023_bot3/control_loops/superstructure/superstructure_goal_generated.h"
-#include "y2023_bot3/control_loops/superstructure/superstructure_status_generated.h"
-
-namespace y2023_bot3 {
-namespace control_loops {
-namespace superstructure {
-
-class EndEffector {
- public:
- static constexpr double kRollerCubeSuckVoltage() { return -7.0; }
- static constexpr double kRollerCubeSpitVoltage() { return 3.0; }
- static constexpr double kRollerCubeSpitMidVoltage() { return 5.0; }
- static constexpr double kRollerCubeSpitHighVoltage() { return 6.37; }
-
- EndEffector();
- void RunIteration(const ::aos::monotonic_clock::time_point timestamp,
- RollerGoal roller_goal, bool beambreak_status,
- double *intake_roller_voltage, bool preloaded_with_cube);
- EndEffectorState state() const { return state_; }
- void Reset();
-
- private:
- EndEffectorState state_;
-
- // variable which records the last time at which "intake" button was pressed
- aos::monotonic_clock::time_point timer_;
-};
-
-} // namespace superstructure
-} // namespace control_loops
-} // namespace y2023_bot3
-
-#endif // Y2023_CONTROL_LOOPS_SUPERSTRUCTURE_END_EFFECTOR_H_
diff --git a/y2023_bot3/control_loops/superstructure/pivot_joint.cc b/y2023_bot3/control_loops/superstructure/pivot_joint.cc
deleted file mode 100644
index be7e646..0000000
--- a/y2023_bot3/control_loops/superstructure/pivot_joint.cc
+++ /dev/null
@@ -1,83 +0,0 @@
-#include "pivot_joint.h"
-
-#include "aos/events/event_loop.h"
-#include "aos/time/time.h"
-#include "frc971/control_loops/control_loop.h"
-#include "y2023_bot3/constants.h"
-#include "y2023_bot3/control_loops/superstructure/superstructure_goal_generated.h"
-#include "y2023_bot3/control_loops/superstructure/superstructure_status_generated.h"
-
-namespace y2023_bot3 {
-namespace control_loops {
-namespace superstructure {
-
-PivotJoint::PivotJoint(std::shared_ptr<const constants::Values> values)
- : pivot_joint_(values->pivot_joint.subsystem_params) {}
-
-flatbuffers::Offset<
- frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>
-PivotJoint::RunIteration(PivotGoal goal, double *output,
- const frc971::PotAndAbsolutePosition *position,
- flatbuffers::FlatBufferBuilder *status_fbb) {
- double pivot_goal = 0;
- switch (goal) {
- case PivotGoal::NEUTRAL:
- pivot_goal = 0;
- break;
-
- case PivotGoal::PICKUP_FRONT:
- pivot_goal = 1.74609993820075;
- break;
-
- case PivotGoal::PICKUP_BACK:
- pivot_goal = -1.7763851077235;
- break;
-
- case PivotGoal::SCORE_LOW_FRONT:
- pivot_goal = 1.74609993820075;
- break;
-
- case PivotGoal::SCORE_LOW_BACK:
- pivot_goal = -1.7763851077235;
- break;
-
- case PivotGoal::SCORE_MID_FRONT:
- pivot_goal = 0.846887672907125;
- break;
-
- case PivotGoal::SCORE_MID_BACK:
- pivot_goal = -0.763222056740831;
- break;
-
- case PivotGoal::SCORE_HIGH_FRONT:
- pivot_goal = 0.846887672907125;
- break;
-
- case PivotGoal::SCORE_HIGH_BACK:
- pivot_goal = -0.763222056740831;
- break;
- }
-
- flatbuffers::Offset<
- frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal>
- pivot_joint_offset = frc971::control_loops::
- CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
- *status_fbb, pivot_goal,
- frc971::CreateProfileParameters(*status_fbb, 5.0, 20.0));
-
- status_fbb->Finish(pivot_joint_offset);
-
- flatbuffers::Offset<
- frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>
- pivot_joint_status_offset = pivot_joint_.Iterate(
- flatbuffers::GetRoot<frc971::control_loops::
- StaticZeroingSingleDOFProfiledSubsystemGoal>(
- status_fbb->GetBufferPointer()),
- position, output, status_fbb);
-
- return pivot_joint_status_offset;
-}
-
-} // namespace superstructure
-} // namespace control_loops
-} // namespace y2023_bot3
diff --git a/y2023_bot3/control_loops/superstructure/pivot_joint.h b/y2023_bot3/control_loops/superstructure/pivot_joint.h
deleted file mode 100644
index 1eff122..0000000
--- a/y2023_bot3/control_loops/superstructure/pivot_joint.h
+++ /dev/null
@@ -1,45 +0,0 @@
-#ifndef Y2023_BOT3_CONTROL_LOOPS_SUPERSTRUCTURE_PIVOT_JOINT_PIVOT_JOINT_H_
-#define Y2023_BOT3_CONTROL_LOOPS_SUPERSTRUCTURE_PIVOT_JOINT_PIVOT_JOINT_H_
-
-#include "aos/events/event_loop.h"
-#include "aos/time/time.h"
-#include "frc971/control_loops/control_loop.h"
-#include "y2023_bot3/constants.h"
-#include "y2023_bot3/control_loops/superstructure/superstructure_goal_generated.h"
-#include "y2023_bot3/control_loops/superstructure/superstructure_status_generated.h"
-
-namespace y2023_bot3 {
-namespace control_loops {
-namespace superstructure {
-
-class PivotJoint {
- using PotAndAbsoluteEncoderSubsystem =
- ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystem<
- ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator,
- ::frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>;
-
- public:
- PivotJoint(std::shared_ptr<const constants::Values> values);
-
- flatbuffers::Offset<
- frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>
- RunIteration(PivotGoal goal, double *output,
- const frc971::PotAndAbsolutePosition *position,
- flatbuffers::FlatBufferBuilder *status_fbb);
-
- bool zeroed() const { return pivot_joint_.zeroed(); }
-
- bool estopped() const { return pivot_joint_.estopped(); }
-
- // variable which records the last time at which "intake" button was pressed
- aos::monotonic_clock::time_point timer_;
-
- private:
- PotAndAbsoluteEncoderSubsystem pivot_joint_;
-};
-
-} // namespace superstructure
-} // namespace control_loops
-} // namespace y2023_bot3
-
-#endif // Y2023_BOT3_CONTROL_LOOPS_SUPERSTRUCTURE_PIVOT_JOINT_PIVOT_JOINT_H_
diff --git a/y2023_bot3/control_loops/superstructure/pivot_joint/BUILD b/y2023_bot3/control_loops/superstructure/pivot_joint/BUILD
deleted file mode 100644
index ae413ab..0000000
--- a/y2023_bot3/control_loops/superstructure/pivot_joint/BUILD
+++ /dev/null
@@ -1,34 +0,0 @@
-package(default_visibility = ["//y2023_bot3:__subpackages__"])
-
-genrule(
- name = "genrule_pivot_joint",
- outs = [
- "pivot_joint_plant.h",
- "pivot_joint_plant.cc",
- "integral_pivot_joint_plant.h",
- "integral_pivot_joint_plant.cc",
- ],
- cmd = "$(location //y2023_bot3/control_loops/python:pivot_joint) $(OUTS)",
- target_compatible_with = ["@platforms//os:linux"],
- tools = [
- "//y2023_bot3/control_loops/python:pivot_joint",
- ],
-)
-
-cc_library(
- name = "pivot_joint_plants",
- srcs = [
- "integral_pivot_joint_plant.cc",
- "pivot_joint_plant.cc",
- ],
- hdrs = [
- "integral_pivot_joint_plant.h",
- "pivot_joint_plant.h",
- ],
- target_compatible_with = ["@platforms//os:linux"],
- visibility = ["//visibility:public"],
- deps = [
- "//frc971/control_loops:hybrid_state_feedback_loop",
- "//frc971/control_loops:state_feedback_loop",
- ],
-)
diff --git a/y2023_bot3/control_loops/superstructure/superstructure.cc b/y2023_bot3/control_loops/superstructure/superstructure.cc
index 6061a76..e77245e 100644
--- a/y2023_bot3/control_loops/superstructure/superstructure.cc
+++ b/y2023_bot3/control_loops/superstructure/superstructure.cc
@@ -23,9 +23,7 @@
const ::std::string &name)
: frc971::controls::ControlLoop<Goal, Position, Status, Output>(event_loop,
name),
- values_(values),
- end_effector_(),
- pivot_joint_(values) {
+ values_(values) {
event_loop->SetRuntimeRealtimePriority(30);
}
@@ -33,41 +31,23 @@
const Position *position,
aos::Sender<Output>::Builder *output,
aos::Sender<Status>::Builder *status) {
- const monotonic_clock::time_point timestamp =
- event_loop()->context().monotonic_event_time;
+ (void)unsafe_goal;
+ (void)position;
if (WasReset()) {
AOS_LOG(ERROR, "WPILib reset, restarting\n");
- end_effector_.Reset();
}
OutputT output_struct;
- end_effector_.RunIteration(
- timestamp,
- unsafe_goal != nullptr ? unsafe_goal->roller_goal() : RollerGoal::IDLE,
- position->end_effector_cube_beam_break(), &output_struct.roller_voltage,
- unsafe_goal != nullptr ? unsafe_goal->preloaded_with_cube() : false);
-
- flatbuffers::Offset<
- frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>
- pivot_joint_offset = pivot_joint_.RunIteration(
- unsafe_goal != nullptr ? unsafe_goal->pivot_goal()
- : PivotGoal::NEUTRAL,
- output != nullptr ? &(output_struct.pivot_joint_voltage) : nullptr,
- position->pivot_joint_position(), status->fbb());
-
Status::Builder status_builder = status->MakeBuilder<Status>();
- status_builder.add_zeroed(pivot_joint_.zeroed());
- status_builder.add_estopped(pivot_joint_.estopped());
- status_builder.add_pivot_joint(pivot_joint_offset);
- status_builder.add_end_effector_state(end_effector_.state());
-
if (output) {
output->CheckOk(output->Send(Output::Pack(*output->fbb(), &output_struct)));
}
+ status_builder.add_zeroed(true);
+
(void)status->Send(status_builder.Finish());
}
diff --git a/y2023_bot3/control_loops/superstructure/superstructure.h b/y2023_bot3/control_loops/superstructure/superstructure.h
index efc95a8..d0cd0db 100644
--- a/y2023_bot3/control_loops/superstructure/superstructure.h
+++ b/y2023_bot3/control_loops/superstructure/superstructure.h
@@ -9,8 +9,6 @@
#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "y2023_bot3/constants.h"
#include "y2023_bot3/constants/constants_generated.h"
-#include "y2023_bot3/control_loops/superstructure/end_effector.h"
-#include "y2023_bot3/control_loops/superstructure/pivot_joint.h"
#include "y2023_bot3/control_loops/superstructure/superstructure_goal_generated.h"
#include "y2023_bot3/control_loops/superstructure/superstructure_output_generated.h"
#include "y2023_bot3/control_loops/superstructure/superstructure_position_generated.h"
@@ -34,8 +32,6 @@
double robot_velocity() const;
- inline const EndEffector &end_effector() const { return end_effector_; }
-
protected:
virtual void RunIteration(const Goal *unsafe_goal, const Position *position,
aos::Sender<Output>::Builder *output,
@@ -46,12 +42,8 @@
std::optional<double> LateralOffsetForTimeOfFlight(double reading);
std::shared_ptr<const constants::Values> values_;
- EndEffector end_effector_;
aos::Alliance alliance_ = aos::Alliance::kInvalid;
-
- PivotJoint pivot_joint_;
-
DISALLOW_COPY_AND_ASSIGN(Superstructure);
};
diff --git a/y2023_bot3/control_loops/superstructure/superstructure_goal.fbs b/y2023_bot3/control_loops/superstructure/superstructure_goal.fbs
index d2b8e0b..326a7b0 100644
--- a/y2023_bot3/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2023_bot3/control_loops/superstructure/superstructure_goal.fbs
@@ -2,30 +2,7 @@
namespace y2023_bot3.control_loops.superstructure;
-enum PivotGoal: ubyte {
- NEUTRAL = 0,
- PICKUP_FRONT = 1,
- PICKUP_BACK = 2,
- SCORE_LOW_FRONT = 3,
- SCORE_LOW_BACK = 4,
- SCORE_MID_FRONT = 5,
- SCORE_MID_BACK = 6,
- SCORE_HIGH_FRONT = 7,
- SCORE_HIGH_BACK = 8,
-}
-
-enum RollerGoal: ubyte {
- IDLE = 0,
- INTAKE_CUBE = 1,
- SPIT = 2,
- SPIT_MID = 3,
- SPIT_HIGH = 4,
-}
-
table Goal {
- pivot_goal:PivotGoal (id: 0);
- roller_goal:RollerGoal (id: 1);
- preloaded_with_cube:bool (id: 2);
}
diff --git a/y2023_bot3/control_loops/superstructure/superstructure_lib_test.cc b/y2023_bot3/control_loops/superstructure/superstructure_lib_test.cc
index 10f8f6e..370ec17 100644
--- a/y2023_bot3/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2023_bot3/control_loops/superstructure/superstructure_lib_test.cc
@@ -31,11 +31,6 @@
using ::frc971::control_loops::PositionSensorSimulator;
using ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal;
using DrivetrainStatus = ::frc971::control_loops::drivetrain::Status;
-using PotAndAbsoluteEncoderSimulator =
- frc971::control_loops::SubsystemSimulator<
- frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus,
- Superstructure::PotAndAbsoluteEncoderSubsystem::State,
- constants::Values::PotAndAbsEncoderConstants>;
// Class which simulates the superstructure and sends out queue messages with
// the position.
@@ -50,15 +45,7 @@
superstructure_status_fetcher_(
event_loop_->MakeFetcher<Status>("/superstructure")),
superstructure_output_fetcher_(
- event_loop_->MakeFetcher<Output>("/superstructure")),
- pivot_joint_(new CappedTestPlant(pivot_joint::MakePivotJointPlant()),
- PositionSensorSimulator(
- values->pivot_joint.subsystem_params.zeroing_constants
- .one_revolution_distance),
- values->pivot_joint, constants::Values::kPivotJointRange(),
- values->pivot_joint.subsystem_params.zeroing_constants
- .measured_absolute_position,
- dt) {
+ event_loop_->MakeFetcher<Output>("/superstructure")) {
(void)values;
phased_loop_handle_ = event_loop_->AddPhasedLoop(
[this](int) {
@@ -66,10 +53,6 @@
if (!first_) {
EXPECT_TRUE(superstructure_output_fetcher_.Fetch());
EXPECT_TRUE(superstructure_status_fetcher_.Fetch());
-
- pivot_joint_.Simulate(
- superstructure_output_fetcher_->pivot_joint_voltage(),
- superstructure_status_fetcher_->pivot_joint());
}
first_ = false;
SendPositionMessage();
@@ -82,38 +65,20 @@
::aos::Sender<Position>::Builder builder =
superstructure_position_sender_.MakeBuilder();
- frc971::PotAndAbsolutePosition::Builder pivot_joint_builder =
- builder.MakeBuilder<frc971::PotAndAbsolutePosition>();
- flatbuffers::Offset<frc971::PotAndAbsolutePosition> pivot_joint_offset =
- pivot_joint_.encoder()->GetSensorValues(&pivot_joint_builder);
-
Position::Builder position_builder = builder.MakeBuilder<Position>();
- position_builder.add_end_effector_cube_beam_break(
- end_effector_cube_beam_break_);
- position_builder.add_pivot_joint_position(pivot_joint_offset);
CHECK_EQ(builder.Send(position_builder.Finish()),
aos::RawSender::Error::kOk);
}
- void set_end_effector_cube_beam_break(bool triggered) {
- end_effector_cube_beam_break_ = triggered;
- }
-
- PotAndAbsoluteEncoderSimulator *pivot_joint() { return &pivot_joint_; }
-
private:
::aos::EventLoop *event_loop_;
::aos::PhasedLoopHandler *phased_loop_handle_ = nullptr;
- bool end_effector_cube_beam_break_ = false;
-
::aos::Sender<Position> superstructure_position_sender_;
::aos::Fetcher<Status> superstructure_status_fetcher_;
::aos::Fetcher<Output> superstructure_output_fetcher_;
- PotAndAbsoluteEncoderSimulator pivot_joint_;
-
bool first_ = true;
};
@@ -167,51 +132,6 @@
ASSERT_TRUE(superstructure_goal_fetcher_.get() != nullptr) << ": No goal";
ASSERT_TRUE(superstructure_status_fetcher_.get() != nullptr)
<< ": No status";
-
- if (superstructure_goal_fetcher_->has_pivot_goal()) {
- double pivot_goal = 0.0;
-
- switch (superstructure_goal_fetcher_->pivot_goal()) {
- case PivotGoal::NEUTRAL:
- pivot_goal = 0;
- break;
-
- case PivotGoal::PICKUP_FRONT:
- pivot_goal = 1.74609993820075;
- break;
-
- case PivotGoal::PICKUP_BACK:
- pivot_goal = -1.7763851077235;
- break;
-
- case PivotGoal::SCORE_LOW_FRONT:
- pivot_goal = 1.74609993820075;
- break;
-
- case PivotGoal::SCORE_LOW_BACK:
- pivot_goal = -1.7763851077235;
- break;
-
- case PivotGoal::SCORE_MID_FRONT:
- pivot_goal = 0.846887672907125;
- break;
-
- case PivotGoal::SCORE_MID_BACK:
- pivot_goal = -0.763222056740831;
- break;
-
- case PivotGoal::SCORE_HIGH_FRONT:
- pivot_goal = 0.846887672907125;
- break;
-
- case PivotGoal::SCORE_HIGH_BACK:
- pivot_goal = -0.763222056740831;
- break;
- }
-
- EXPECT_NEAR(pivot_goal,
- superstructure_status_fetcher_->pivot_joint()->position(), 3);
- }
}
void CheckIfZeroed() {
@@ -323,323 +243,6 @@
CheckIfZeroed();
}
-TEST_F(SuperstructureTest, PivotGoal) {
- SetEnabled(true);
- WaitUntilZeroed();
-
- PivotGoal pivot_goal = PivotGoal::PICKUP_FRONT;
-
- {
- auto builder = superstructure_goal_sender_.MakeBuilder();
-
- Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_pivot_goal(pivot_goal);
-
- builder.CheckOk(builder.Send(goal_builder.Finish()));
- }
-
- RunFor(dt() * 30);
-
- ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
- ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
-
- VerifyNearGoal();
-
- pivot_goal = PivotGoal::PICKUP_BACK;
-
- {
- auto builder = superstructure_goal_sender_.MakeBuilder();
-
- Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_pivot_goal(pivot_goal);
-
- builder.CheckOk(builder.Send(goal_builder.Finish()));
- }
-
- RunFor(dt() * 30);
-
- ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
- ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
-
- VerifyNearGoal();
-
- pivot_goal = PivotGoal::SCORE_LOW_FRONT;
-
- {
- auto builder = superstructure_goal_sender_.MakeBuilder();
-
- Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_pivot_goal(pivot_goal);
-
- builder.CheckOk(builder.Send(goal_builder.Finish()));
- }
-
- RunFor(dt() * 30);
-
- ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
- ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
-
- VerifyNearGoal();
-
- pivot_goal = PivotGoal::SCORE_LOW_BACK;
-
- {
- auto builder = superstructure_goal_sender_.MakeBuilder();
-
- Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_pivot_goal(pivot_goal);
-
- builder.CheckOk(builder.Send(goal_builder.Finish()));
- }
-
- RunFor(dt() * 30);
-
- ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
- ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
-
- VerifyNearGoal();
-
- pivot_goal = PivotGoal::SCORE_MID_FRONT;
-
- {
- auto builder = superstructure_goal_sender_.MakeBuilder();
-
- Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_pivot_goal(pivot_goal);
-
- builder.CheckOk(builder.Send(goal_builder.Finish()));
- }
-
- RunFor(dt() * 30);
-
- ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
- ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
-
- VerifyNearGoal();
-
- pivot_goal = PivotGoal::SCORE_MID_BACK;
-
- {
- auto builder = superstructure_goal_sender_.MakeBuilder();
-
- Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_pivot_goal(pivot_goal);
-
- builder.CheckOk(builder.Send(goal_builder.Finish()));
- }
-
- RunFor(dt() * 30);
-
- ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
- ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
-
- VerifyNearGoal();
-
- pivot_goal = PivotGoal::NEUTRAL;
-
- {
- auto builder = superstructure_goal_sender_.MakeBuilder();
-
- Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_pivot_goal(pivot_goal);
-
- builder.CheckOk(builder.Send(goal_builder.Finish()));
- }
-
- RunFor(dt() * 30);
-
- ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
- ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
-
- VerifyNearGoal();
-}
-
-TEST_F(SuperstructureTest, EndEffectorGoal) {
- SetEnabled(true);
- WaitUntilZeroed();
-
- double spit_voltage = EndEffector::kRollerCubeSpitVoltage();
- double suck_voltage = EndEffector::kRollerCubeSuckVoltage();
-
- RollerGoal roller_goal = RollerGoal::INTAKE_CUBE;
-
- {
- auto builder = superstructure_goal_sender_.MakeBuilder();
-
- Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_roller_goal(roller_goal);
-
- builder.CheckOk(builder.Send(goal_builder.Finish()));
- }
- superstructure_plant_.set_end_effector_cube_beam_break(false);
-
- // This makes sure that we intake as normal when
- // requesting intake.
- RunFor(constants::Values::kExtraIntakingTime());
-
- ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
- ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
-
- EXPECT_EQ(superstructure_output_fetcher_->roller_voltage(), suck_voltage);
- EXPECT_EQ(superstructure_status_fetcher_->end_effector_state(),
- EndEffectorState::INTAKING);
-
- superstructure_plant_.set_end_effector_cube_beam_break(true);
-
- // Checking that after the beambreak is set once intaking that the
- // state changes to LOADED.
- RunFor(dt());
-
- ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
- ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
-
- EXPECT_EQ(superstructure_output_fetcher_->roller_voltage(), 0.0);
- EXPECT_EQ(superstructure_status_fetcher_->end_effector_state(),
- EndEffectorState::LOADED);
-
- {
- auto builder = superstructure_goal_sender_.MakeBuilder();
-
- Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_roller_goal(RollerGoal::IDLE);
-
- builder.CheckOk(builder.Send(goal_builder.Finish()));
- }
- superstructure_plant_.set_end_effector_cube_beam_break(false);
-
- // Checking that it's going back to intaking because we lost the
- // beambreak sensor.
- RunFor(dt() * 2);
-
- ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
- ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
-
- EXPECT_EQ(superstructure_output_fetcher_->roller_voltage(), suck_voltage);
- EXPECT_EQ(superstructure_status_fetcher_->end_effector_state(),
- EndEffectorState::INTAKING);
-
- // Checking that we go back to idle after beambreak is lost and we
- // set our goal to idle.
- RunFor(dt() * 2 + constants::Values::kExtraIntakingTime());
- ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
- ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
-
- EXPECT_EQ(superstructure_output_fetcher_->roller_voltage(), 0.0);
- EXPECT_EQ(superstructure_status_fetcher_->end_effector_state(),
- EndEffectorState::IDLE);
-
- {
- auto builder = superstructure_goal_sender_.MakeBuilder();
-
- Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_roller_goal(roller_goal);
-
- builder.CheckOk(builder.Send(goal_builder.Finish()));
- }
-
- // Going through intake -> loaded -> spitting
- // Making sure that it's intaking normally.
- RunFor(constants::Values::kExtraIntakingTime());
-
- ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
- ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
-
- EXPECT_EQ(superstructure_output_fetcher_->roller_voltage(), suck_voltage);
- EXPECT_EQ(superstructure_status_fetcher_->end_effector_state(),
- EndEffectorState::INTAKING);
-
- superstructure_plant_.set_end_effector_cube_beam_break(true);
-
- // Checking that it's loaded once beambreak is sensing something.
- RunFor(dt());
-
- ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
- ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
-
- EXPECT_EQ(superstructure_output_fetcher_->roller_voltage(), 0.0);
- EXPECT_EQ(superstructure_status_fetcher_->end_effector_state(),
- EndEffectorState::LOADED);
-
- {
- auto builder = superstructure_goal_sender_.MakeBuilder();
-
- Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_roller_goal(RollerGoal::SPIT);
-
- builder.CheckOk(builder.Send(goal_builder.Finish()));
- }
- superstructure_plant_.set_end_effector_cube_beam_break(true);
- // Checking that it stays spitting until 2 seconds after the
- // beambreak is lost.
- RunFor(dt() * 10);
-
- ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
- ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
-
- EXPECT_EQ(superstructure_output_fetcher_->roller_voltage(), spit_voltage);
- EXPECT_EQ(superstructure_status_fetcher_->end_effector_state(),
- EndEffectorState::SPITTING);
-
- {
- auto builder = superstructure_goal_sender_.MakeBuilder();
-
- Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
-
- goal_builder.add_roller_goal(RollerGoal::IDLE);
-
- builder.CheckOk(builder.Send(goal_builder.Finish()));
- }
-
- // Checking that it goes to idle after it's given time to stop spitting.
- RunFor(dt() * 3);
-
- ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
- ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
-
- EXPECT_EQ(superstructure_output_fetcher_->roller_voltage(), 0.0);
- EXPECT_EQ(superstructure_status_fetcher_->end_effector_state(),
- EndEffectorState::IDLE);
-}
-
-// Test that we are able to signal that the cube was preloaded
-TEST_F(SuperstructureTest, Preloaded) {
- SetEnabled(true);
- WaitUntilZeroed();
-
- {
- auto builder = superstructure_goal_sender_.MakeBuilder();
- Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_preloaded_with_cube(true);
- ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
- }
-
- RunFor(dt());
-
- ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
- EXPECT_EQ(superstructure_status_fetcher_->end_effector_state(),
- EndEffectorState::LOADED);
-}
-
-// Tests that the end effector does nothing when the goal is to remain
-// still.
-TEST_F(SuperstructureTest, DoesNothing) {
- SetEnabled(true);
- WaitUntilZeroed();
-
- {
- auto builder = superstructure_goal_sender_.MakeBuilder();
-
- Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
-
- goal_builder.add_roller_goal(RollerGoal::IDLE);
-
- ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
- }
- RunFor(chrono::seconds(10));
- VerifyNearGoal();
-
- EXPECT_TRUE(superstructure_output_fetcher_.Fetch());
-}
// Tests that loops can reach a goal.
TEST_F(SuperstructureTest, ReachesGoal) {
SetEnabled(true);
@@ -649,8 +252,6 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_roller_goal(RollerGoal::IDLE);
-
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
// Give it a lot of time to get there.
diff --git a/y2023_bot3/control_loops/superstructure/superstructure_output.fbs b/y2023_bot3/control_loops/superstructure/superstructure_output.fbs
index 652d247..98dbcf7 100644
--- a/y2023_bot3/control_loops/superstructure/superstructure_output.fbs
+++ b/y2023_bot3/control_loops/superstructure/superstructure_output.fbs
@@ -1,10 +1,6 @@
namespace y2023_bot3.control_loops.superstructure;
table Output {
- // Pivot joint voltage
- pivot_joint_voltage:double (id: 0);
- // Roller voltage on the end effector (positive is spitting, negative is sucking)
- roller_voltage:double (id: 1);
}
root_type Output;
diff --git a/y2023_bot3/control_loops/superstructure/superstructure_position.fbs b/y2023_bot3/control_loops/superstructure/superstructure_position.fbs
index a629e56..28e4849 100644
--- a/y2023_bot3/control_loops/superstructure/superstructure_position.fbs
+++ b/y2023_bot3/control_loops/superstructure/superstructure_position.fbs
@@ -5,14 +5,6 @@
namespace y2023_bot3.control_loops.superstructure;
table Position {
- // The position of the pivot joint in radians
- pivot_joint_position:frc971.PotAndAbsolutePosition (id: 0);
-
- // If this is true, the cube beam break is triggered.
- end_effector_cube_beam_break:bool (id: 1);
-
- // Roller falcon data
- roller_falcon:frc971.control_loops.CANFalcon (id: 2);
}
root_type Position;
diff --git a/y2023_bot3/control_loops/superstructure/superstructure_status.fbs b/y2023_bot3/control_loops/superstructure/superstructure_status.fbs
index 4492cb9..b36e0da 100644
--- a/y2023_bot3/control_loops/superstructure/superstructure_status.fbs
+++ b/y2023_bot3/control_loops/superstructure/superstructure_status.fbs
@@ -3,35 +3,12 @@
namespace y2023_bot3.control_loops.superstructure;
-enum EndEffectorState : ubyte {
- // Not doing anything
- IDLE = 0,
- // Intaking the game object into the end effector
- INTAKING = 1,
- // The game object is loaded into the end effector
- LOADED = 2,
- // Waiting for the arm to be at shooting goal and then telling the
- // end effector to spit
- SPITTING = 3,
- // Waiting for the arm to be at MID shooting goal and then telling the
- // end effector to spit mid
- SPITTING_MID = 4,
- // Waiting for the arm to be at HIGH shooting goal and then telling the
- // end effector to spit mid
- SPITTING_HIGH = 5
-}
-
table Status {
// All subsystems know their location.
zeroed:bool (id: 0);
// If true, we have aborted. This is the or of all subsystem estops.
estopped:bool (id: 1);
-
- // The current state of the pivot.
- pivot_joint:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus (id: 2);
-
- end_effector_state:EndEffectorState (id: 3);
}
root_type Status;
diff --git a/y2023_bot3/joystick_reader.cc b/y2023_bot3/joystick_reader.cc
index fd32652..4c42bdc 100644
--- a/y2023_bot3/joystick_reader.cc
+++ b/y2023_bot3/joystick_reader.cc
@@ -31,8 +31,6 @@
using frc971::input::driver_station::JoystickAxis;
using frc971::input::driver_station::POVLocation;
using Side = frc971::control_loops::drivetrain::RobotSide;
-using y2023_bot3::control_loops::superstructure::PivotGoal;
-using y2023_bot3::control_loops::superstructure::RollerGoal;
namespace y2023_bot3 {
namespace input {
@@ -90,33 +88,6 @@
superstructure::Goal::Builder superstructure_goal_builder =
builder.MakeBuilder<superstructure::Goal>();
- RollerGoal roller_goal = RollerGoal::IDLE;
- PivotGoal pivot_goal = PivotGoal::NEUTRAL;
-
- if (data.IsPressed(kSpit)) {
- roller_goal = RollerGoal::SPIT;
- } else if (data.IsPressed(kSpitHigh)) {
- roller_goal = RollerGoal::SPIT_HIGH;
- }
-
- if (data.IsPressed(kScore)) {
- pivot_goal = PivotGoal::SCORE_LOW_FRONT;
- } else if (data.IsPressed(kScoreBack)) {
- pivot_goal = PivotGoal::SCORE_LOW_BACK;
- } else if (data.IsPressed(kScoreMid)) {
- pivot_goal = PivotGoal::SCORE_MID_FRONT;
- } else if (data.IsPressed(kScoreMidBack)) {
- pivot_goal = PivotGoal::SCORE_MID_BACK;
- } else if (data.IsPressed(kPickup)) {
- pivot_goal = PivotGoal::PICKUP_FRONT;
- roller_goal = RollerGoal::INTAKE_CUBE;
- } else if (data.IsPressed(kPickupBack)) {
- pivot_goal = PivotGoal::PICKUP_BACK;
- roller_goal = RollerGoal::INTAKE_CUBE;
- }
-
- superstructure_goal_builder.add_roller_goal(roller_goal);
- superstructure_goal_builder.add_pivot_goal(pivot_goal);
if (builder.Send(superstructure_goal_builder.Finish()) !=
aos::RawSender::Error::kOk) {
AOS_LOG(ERROR, "Sending superstructure goal failed.\n");
@@ -146,4 +117,4 @@
event_loop.Run();
return 0;
-}
\ No newline at end of file
+}
diff --git a/y2023_bot3/wpilib_interface.cc b/y2023_bot3/wpilib_interface.cc
index e91f6e7..9b8df5d 100644
--- a/y2023_bot3/wpilib_interface.cc
+++ b/y2023_bot3/wpilib_interface.cc
@@ -88,13 +88,8 @@
control_loops::drivetrain::kWheelRadius;
}
-double pivot_pot_translate(double voltage) {
- return voltage * Values::kPivotJointPotRadiansPerVolt();
-}
-
constexpr double kMaxFastEncoderPulsesPerSecond = std::max({
Values::kMaxDrivetrainEncoderPulsesPerSecond(),
- Values::kMaxPivotJointEncoderPulsesPerSecond(),
});
static_assert(kMaxFastEncoderPulsesPerSecond <= 1300000,
"fast encoders are too fast");
@@ -180,62 +175,6 @@
PrintConfigs();
}
- void WriteRollerConfigs() {
- ctre::phoenix6::configs::CurrentLimitsConfigs current_limits;
- current_limits.StatorCurrentLimit =
- constants::Values::kRollerStatorCurrentLimit();
- current_limits.StatorCurrentLimitEnable = true;
- current_limits.SupplyCurrentLimit =
- constants::Values::kRollerSupplyCurrentLimit();
- current_limits.SupplyCurrentLimitEnable = true;
-
- ctre::phoenix6::configs::MotorOutputConfigs output_configs;
- output_configs.NeutralMode =
- ctre::phoenix6::signals::NeutralModeValue::Brake;
- output_configs.DutyCycleNeutralDeadband = 0;
-
- ctre::phoenix6::configs::TalonFXConfiguration configuration;
- configuration.CurrentLimits = current_limits;
- configuration.MotorOutput = output_configs;
-
- ctre::phoenix::StatusCode status =
- talon_.GetConfigurator().Apply(configuration);
- if (!status.IsOK()) {
- AOS_LOG(ERROR, "Failed to set falcon configuration: %s: %s",
- status.GetName(), status.GetDescription());
- }
-
- PrintConfigs();
- }
-
- void WritePivotConfigs() {
- ctre::phoenix6::configs::CurrentLimitsConfigs current_limits;
- current_limits.StatorCurrentLimit =
- constants::Values::kPivotStatorCurrentLimit();
- current_limits.StatorCurrentLimitEnable = true;
- current_limits.SupplyCurrentLimit =
- constants::Values::kPivotSupplyCurrentLimit();
- current_limits.SupplyCurrentLimitEnable = true;
-
- ctre::phoenix6::configs::MotorOutputConfigs output_configs;
- output_configs.NeutralMode =
- ctre::phoenix6::signals::NeutralModeValue::Brake;
- output_configs.DutyCycleNeutralDeadband = 0;
-
- ctre::phoenix6::configs::TalonFXConfiguration configuration;
- configuration.CurrentLimits = current_limits;
- configuration.MotorOutput = output_configs;
-
- ctre::phoenix::StatusCode status =
- talon_.GetConfigurator().Apply(configuration);
- if (!status.IsOK()) {
- AOS_LOG(ERROR, "Failed to set falcon configuration: %s: %s",
- status.GetName(), status.GetDescription());
- }
-
- PrintConfigs();
- }
-
ctre::phoenix6::hardware::TalonFX *talon() { return &talon_; }
flatbuffers::Offset<frc971::control_loops::CANFalcon> WritePosition(
@@ -302,8 +241,7 @@
can_position_sender_(
event_loop
->MakeSender<frc971::control_loops::drivetrain::CANPosition>(
- "/drivetrain")),
- roller_falcon_data_(std::nullopt) {
+ "/drivetrain")) {
event_loop->SetRuntimeRealtimePriority(40);
event_loop->SetRuntimeAffinity(aos::MakeCpusetFromCpus({1}));
timer_handler_ = event_loop->AddTimer([this]() { Loop(); });
@@ -316,22 +254,9 @@
}
void set_falcons(std::shared_ptr<Falcon> right_front,
- std::shared_ptr<Falcon> right_back,
- std::shared_ptr<Falcon> left_front,
- std::shared_ptr<Falcon> left_back,
- std::shared_ptr<Falcon> roller_falcon,
- std::shared_ptr<Falcon> pivot_falcon) {
+ std::shared_ptr<Falcon> right_back) {
right_front_ = std::move(right_front);
right_back_ = std::move(right_back);
- left_front_ = std::move(left_front);
- left_back_ = std::move(left_back);
- roller_falcon_ = std::move(roller_falcon);
- pivot_falcon_ = std::move(pivot_falcon);
- }
-
- std::optional<frc971::control_loops::CANFalconT> roller_falcon_data() {
- std::unique_lock<aos::stl_mutex> lock(roller_mutex_);
- return roller_falcon_data_;
}
private:
@@ -346,8 +271,7 @@
auto builder = can_position_sender_.MakeBuilder();
- for (auto falcon : {right_front_, right_back_, left_front_, left_back_,
- roller_falcon_, pivot_falcon_}) {
+ for (auto falcon : {right_front_, right_back_}) {
falcon->RefreshNontimesyncedSignals();
}
@@ -355,7 +279,7 @@
kCANFalconCount>
falcons;
- for (auto falcon : {right_front_, right_back_, left_front_, left_back_}) {
+ for (auto falcon : {right_front_, right_back_}) {
falcons.push_back(falcon->WritePosition(builder.fbb()));
}
@@ -374,21 +298,6 @@
can_position_builder.add_status(static_cast<int>(status));
builder.CheckOk(builder.Send(can_position_builder.Finish()));
-
- {
- std::unique_lock<aos::stl_mutex> lock(roller_mutex_);
- frc971::control_loops::CANFalconT roller_falcon_data;
- roller_falcon_data.id = roller_falcon_->device_id();
- roller_falcon_data.supply_current = roller_falcon_->supply_current();
- roller_falcon_data.torque_current = -roller_falcon_->torque_current();
- roller_falcon_data.supply_voltage = roller_falcon_->supply_voltage();
- roller_falcon_data.device_temp = roller_falcon_->device_temp();
- roller_falcon_data.position = -roller_falcon_->position();
- roller_falcon_data.duty_cycle = roller_falcon_->duty_cycle();
- roller_falcon_data_ =
- std::make_optional<frc971::control_loops::CANFalconT>(
- roller_falcon_data);
- }
}
aos::EventLoop *event_loop_;
@@ -397,12 +306,7 @@
aos::Sender<frc971::control_loops::drivetrain::CANPosition>
can_position_sender_;
- std::shared_ptr<Falcon> right_front_, right_back_, left_front_, left_back_,
- roller_falcon_, pivot_falcon_;
-
- std::optional<frc971::control_loops::CANFalconT> roller_falcon_data_;
-
- aos::stl_mutex roller_mutex_;
+ std::shared_ptr<Falcon> right_front_, right_back_;
// Pointer to the timer handler used to modify the wakeup.
::aos::TimerHandler *timer_handler_;
@@ -452,32 +356,8 @@
{
auto builder = superstructure_position_sender_.MakeBuilder();
- flatbuffers::Offset<frc971::control_loops::CANFalcon>
- roller_falcon_offset;
- frc971::PotAndAbsolutePositionT pivot;
- CopyPosition(pivot_encoder_, &pivot,
- Values::kPivotJointEncoderCountsPerRevolution(),
- Values::kPivotJointEncoderRatio(), pivot_pot_translate, true,
- values_->pivot_joint.potentiometer_offset);
-
- auto optional_roller_falcon = can_sensor_reader_->roller_falcon_data();
- if (optional_roller_falcon.has_value()) {
- roller_falcon_offset = frc971::control_loops::CANFalcon::Pack(
- *builder.fbb(), &optional_roller_falcon.value());
- }
-
- flatbuffers::Offset<frc971::PotAndAbsolutePosition> pivot_offset =
- frc971::PotAndAbsolutePosition::Pack(*builder.fbb(), &pivot);
-
superstructure::Position::Builder position_builder =
builder.MakeBuilder<superstructure::Position>();
- position_builder.add_end_effector_cube_beam_break(
- !end_effector_cube_beam_break_->Get());
- position_builder.add_pivot_joint_position(pivot_offset);
-
- if (!roller_falcon_offset.IsNull()) {
- position_builder.add_roller_falcon(roller_falcon_offset);
- }
builder.CheckOk(builder.Send(position_builder.Finish()));
}
@@ -556,26 +436,6 @@
superstructure_reading_ = superstructure_reading;
}
- void set_end_effector_cube_beam_break(
- ::std::unique_ptr<frc::DigitalInput> sensor) {
- end_effector_cube_beam_break_ = ::std::move(sensor);
- }
-
- void set_pivot_encoder(::std::unique_ptr<frc::Encoder> encoder) {
- fast_encoder_filter_.Add(encoder.get());
- pivot_encoder_.set_encoder(::std::move(encoder));
- }
-
- void set_pivot_absolute_pwm(
- ::std::unique_ptr<frc::DigitalInput> absolute_pwm) {
- pivot_encoder_.set_absolute_pwm(::std::move(absolute_pwm));
- }
-
- void set_pivot_potentiometer(
- ::std::unique_ptr<frc::AnalogInput> potentiometer) {
- pivot_encoder_.set_potentiometer(::std::move(potentiometer));
- }
-
private:
std::shared_ptr<const Values> values_;
@@ -587,95 +447,12 @@
std::array<std::unique_ptr<frc::DigitalInput>, 2> autonomous_modes_;
- std::unique_ptr<frc::DigitalInput> imu_yaw_rate_input_,
- end_effector_cube_beam_break_;
+ std::unique_ptr<frc::DigitalInput> imu_yaw_rate_input_;
frc971::wpilib::DMAPulseWidthReader imu_yaw_rate_reader_;
- frc971::wpilib::AbsoluteEncoderAndPotentiometer pivot_encoder_;
-
CANSensorReader *can_sensor_reader_;
};
-
-class SuperstructureCANWriter
- : public ::frc971::wpilib::LoopOutputHandler<superstructure::Output> {
- public:
- SuperstructureCANWriter(::aos::EventLoop *event_loop)
- : ::frc971::wpilib::LoopOutputHandler<superstructure::Output>(
- event_loop, "/superstructure") {
- event_loop->SetRuntimeRealtimePriority(
- constants::Values::kSuperstructureCANWriterPriority);
-
- event_loop->OnRun([this]() { WriteConfigs(); });
- };
-
- void HandleCANConfiguration(const frc971::CANConfiguration &configuration) {
- roller_falcon_->PrintConfigs();
- pivot_falcon_->PrintConfigs();
- if (configuration.reapply()) {
- WriteConfigs();
- }
- }
-
- void set_roller_falcon(std::shared_ptr<Falcon> roller_falcon) {
- roller_falcon_ = std::move(roller_falcon);
- }
-
- void set_pivot_falcon(std::shared_ptr<Falcon> pivot_falcon) {
- pivot_falcon_ = std::move(pivot_falcon);
- }
-
- private:
- void WriteConfigs() {
- roller_falcon_->WriteRollerConfigs();
- pivot_falcon_->WritePivotConfigs();
- }
-
- void Write(const superstructure::Output &output) override {
- ctre::phoenix6::controls::DutyCycleOut roller_control(
- SafeSpeed(-output.roller_voltage()));
- roller_control.UpdateFreqHz = 0_Hz;
- roller_control.EnableFOC = true;
-
- ctre::phoenix6::controls::DutyCycleOut pivot_control(
- SafeSpeed(output.pivot_joint_voltage()));
- pivot_control.UpdateFreqHz = 0_Hz;
- pivot_control.EnableFOC = true;
-
- ctre::phoenix::StatusCode status =
- roller_falcon_->talon()->SetControl(roller_control);
-
- if (!status.IsOK()) {
- AOS_LOG(ERROR, "Failed to write control to falcon: %s: %s",
- status.GetName(), status.GetDescription());
- }
-
- status = pivot_falcon_->talon()->SetControl(pivot_control);
-
- if (!status.IsOK()) {
- AOS_LOG(ERROR, "Failed to write control to falcon: %s: %s",
- status.GetName(), status.GetDescription());
- }
- }
-
- void Stop() override {
- AOS_LOG(WARNING, "Superstructure CAN output too old.\n");
- ctre::phoenix6::controls::DutyCycleOut stop_command(0.0);
- stop_command.UpdateFreqHz = 0_Hz;
- stop_command.EnableFOC = true;
-
- roller_falcon_->talon()->SetControl(stop_command);
- pivot_falcon_->talon()->SetControl(stop_command);
- }
-
- double SafeSpeed(double voltage) {
- return (::aos::Clip(voltage, -kMaxBringupPower, kMaxBringupPower) / 12.0);
- }
-
- std::shared_ptr<Falcon> roller_falcon_;
- std::shared_ptr<Falcon> pivot_falcon_;
-};
-
class DrivetrainWriter : public ::frc971::wpilib::LoopOutputHandler<
::frc971::control_loops::drivetrain::Output> {
public:
@@ -690,25 +467,17 @@
}
void set_falcons(std::shared_ptr<Falcon> right_front,
- std::shared_ptr<Falcon> right_back,
- std::shared_ptr<Falcon> left_front,
- std::shared_ptr<Falcon> left_back) {
+ std::shared_ptr<Falcon> right_back) {
right_front_ = std::move(right_front);
right_back_ = std::move(right_back);
- left_front_ = std::move(left_front);
- left_back_ = std::move(left_back);
}
void set_right_inverted(ctre::phoenix6::signals::InvertedValue invert) {
right_inverted_ = invert;
}
- void set_left_inverted(ctre::phoenix6::signals::InvertedValue invert) {
- left_inverted_ = invert;
- }
-
void HandleCANConfiguration(const frc971::CANConfiguration &configuration) {
- for (auto falcon : {right_front_, right_back_, left_front_, left_back_}) {
+ for (auto falcon : {right_front_, right_back_}) {
falcon->PrintConfigs();
}
if (configuration.reapply()) {
@@ -721,34 +490,15 @@
for (auto falcon : {right_front_.get(), right_back_.get()}) {
falcon->WriteConfigs(right_inverted_);
}
-
- for (auto falcon : {left_front_.get(), left_back_.get()}) {
- falcon->WriteConfigs(left_inverted_);
- }
}
void Write(
const ::frc971::control_loops::drivetrain::Output &output) override {
- ctre::phoenix6::controls::DutyCycleOut left_control(
- SafeSpeed(output.left_voltage()));
- left_control.UpdateFreqHz = 0_Hz;
- left_control.EnableFOC = true;
-
ctre::phoenix6::controls::DutyCycleOut right_control(
SafeSpeed(output.right_voltage()));
right_control.UpdateFreqHz = 0_Hz;
right_control.EnableFOC = true;
- for (auto falcon : {left_front_.get(), left_back_.get()}) {
- ctre::phoenix::StatusCode status =
- falcon->talon()->SetControl(left_control);
-
- if (!status.IsOK()) {
- AOS_LOG(ERROR, "Failed to write control to falcon: %s: %s",
- status.GetName(), status.GetDescription());
- }
- }
-
for (auto falcon : {right_front_.get(), right_back_.get()}) {
ctre::phoenix::StatusCode status =
falcon->talon()->SetControl(right_control);
@@ -766,8 +516,7 @@
stop_command.UpdateFreqHz = 0_Hz;
stop_command.EnableFOC = true;
- for (auto falcon : {right_front_.get(), right_back_.get(),
- left_front_.get(), left_back_.get()}) {
+ for (auto falcon : {right_front_.get(), right_back_.get()}) {
falcon->talon()->SetControl(stop_command);
}
}
@@ -776,8 +525,58 @@
return (::aos::Clip(voltage, -kMaxBringupPower, kMaxBringupPower) / 12.0);
}
- ctre::phoenix6::signals::InvertedValue left_inverted_, right_inverted_;
- std::shared_ptr<Falcon> right_front_, right_back_, left_front_, left_back_;
+ ctre::phoenix6::signals::InvertedValue right_inverted_;
+ std::shared_ptr<Falcon> right_front_, right_back_;
+};
+class PWMDrivetrainWriter : public ::frc971::wpilib::LoopOutputHandler<
+ ::frc971::control_loops::drivetrain::Output> {
+ public:
+ static constexpr double kMaxBringupPower = 12.0;
+
+ PWMDrivetrainWriter(::aos::EventLoop *event_loop)
+ : ::frc971::wpilib::LoopOutputHandler<
+ ::frc971::control_loops::drivetrain::Output>(event_loop,
+ "/drivetrain") {}
+
+ void set_left_controller0(::std::unique_ptr<::frc::PWM> t, bool reversed) {
+ left_controller0_ = ::std::move(t);
+ reversed_left0_ = reversed;
+ }
+
+ void set_left_controller1(::std::unique_ptr<::frc::PWM> t, bool reversed) {
+ left_controller1_ = ::std::move(t);
+ reversed_left1_ = reversed;
+ }
+
+ private:
+ void Write(
+ const ::frc971::control_loops::drivetrain::Output &output) override {
+ left_controller0_->SetSpeed(
+ SafeSpeed(reversed_left0_, output.left_voltage()));
+
+ if (left_controller1_) {
+ left_controller1_->SetSpeed(
+ SafeSpeed(reversed_left1_, output.left_voltage()));
+ }
+ }
+ void Stop() override {
+ AOS_LOG(WARNING, "drivetrain output too old\n");
+ left_controller0_->SetDisabled();
+
+ if (left_controller1_) {
+ left_controller1_->SetDisabled();
+ }
+ }
+
+ double SafeSpeed(bool reversed, double voltage) {
+ return (::aos::Clip((reversed ? -1.0 : 1.0) * voltage, -kMaxBringupPower,
+ kMaxBringupPower) /
+ 12.0);
+ }
+
+ ::std::unique_ptr<::frc::PWM> left_controller0_, left_controller1_;
+
+ bool reversed_right0_, reversed_left0_, reversed_right1_, reversed_left1_;
};
class WPILibRobot : public ::frc971::wpilib::WPILibRobotBase {
@@ -813,46 +612,36 @@
std::make_shared<Falcon>(1, "Drivetrain Bus", &signals_registry);
std::shared_ptr<Falcon> right_back =
std::make_shared<Falcon>(0, "Drivetrain Bus", &signals_registry);
- std::shared_ptr<Falcon> left_front =
- std::make_shared<Falcon>(2, "Drivetrain Bus", &signals_registry);
- std::shared_ptr<Falcon> left_back =
- std::make_shared<Falcon>(3, "Drivetrain Bus", &signals_registry);
- std::shared_ptr<Falcon> roller =
- std::make_shared<Falcon>(5, "Drivetrain Bus", &signals_registry);
- std::shared_ptr<Falcon> pivot =
- std::make_shared<Falcon>(4, "Drivetrain Bus", &signals_registry);
// Thread 3.
+ ::aos::ShmEventLoop output_event_loop(&config.message());
+ PWMDrivetrainWriter drivetrain_writer(&output_event_loop);
+ drivetrain_writer.set_left_controller0(
+ ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(0)), true);
+
+ AddLoop(&output_event_loop);
+
+ // Thread 4
::aos::ShmEventLoop can_sensor_reader_event_loop(&config.message());
can_sensor_reader_event_loop.set_name("CANSensorReader");
CANSensorReader can_sensor_reader(&can_sensor_reader_event_loop,
std::move(signals_registry));
-
- can_sensor_reader.set_falcons(right_front, right_back, left_front,
- left_back, roller, pivot);
-
+ can_sensor_reader.set_falcons(right_front, right_back);
AddLoop(&can_sensor_reader_event_loop);
- // Thread 4.
+ // Thread 5.
::aos::ShmEventLoop sensor_reader_event_loop(&config.message());
SensorReader sensor_reader(&sensor_reader_event_loop, values,
&can_sensor_reader);
sensor_reader.set_pwm_trigger(true);
- sensor_reader.set_drivetrain_left_encoder(make_encoder(4));
- sensor_reader.set_drivetrain_right_encoder(make_encoder(5));
+ sensor_reader.set_drivetrain_left_encoder(make_encoder(1));
+ sensor_reader.set_drivetrain_right_encoder(make_encoder(0));
sensor_reader.set_superstructure_reading(superstructure_reading);
sensor_reader.set_yaw_rate_input(make_unique<frc::DigitalInput>(3));
- sensor_reader.set_end_effector_cube_beam_break(
- make_unique<frc::DigitalInput>(22));
-
- sensor_reader.set_pivot_encoder(make_encoder(0));
- sensor_reader.set_pivot_absolute_pwm(make_unique<frc::DigitalInput>(0));
- sensor_reader.set_pivot_potentiometer(make_unique<frc::AnalogInput>(0));
-
AddLoop(&sensor_reader_event_loop);
- // Thread 5.
+ // Thread 6.
// Set up CAN.
if (!FLAGS_ctre_diag_server) {
c_Phoenix_Diagnostics_SetSecondsToStart(-1);
@@ -866,24 +655,16 @@
::aos::ShmEventLoop can_output_event_loop(&config.message());
can_output_event_loop.set_name("CANOutputWriter");
- DrivetrainWriter drivetrain_writer(&can_output_event_loop);
+ DrivetrainWriter can_drivetrain_writer(&can_output_event_loop);
- drivetrain_writer.set_falcons(right_front, right_back, left_front,
- left_back);
- drivetrain_writer.set_right_inverted(
+ can_drivetrain_writer.set_falcons(right_front, right_back);
+ can_drivetrain_writer.set_right_inverted(
ctre::phoenix6::signals::InvertedValue::CounterClockwise_Positive);
- drivetrain_writer.set_left_inverted(
- ctre::phoenix6::signals::InvertedValue::Clockwise_Positive);
-
- SuperstructureCANWriter superstructure_can_writer(&can_output_event_loop);
- superstructure_can_writer.set_roller_falcon(roller);
- superstructure_can_writer.set_pivot_falcon(pivot);
can_output_event_loop.MakeWatcher(
- "/roborio", [&drivetrain_writer, &superstructure_can_writer](
+ "/roborio", [&can_drivetrain_writer](
const frc971::CANConfiguration &configuration) {
- drivetrain_writer.HandleCANConfiguration(configuration);
- superstructure_can_writer.HandleCANConfiguration(configuration);
+ can_drivetrain_writer.HandleCANConfiguration(configuration);
});
AddLoop(&can_output_event_loop);
diff --git a/y2024_defense/BUILD b/y2024_defense/BUILD
new file mode 100644
index 0000000..12eeec3
--- /dev/null
+++ b/y2024_defense/BUILD
@@ -0,0 +1,238 @@
+load("//frc971:downloader.bzl", "robot_downloader")
+load("//aos:config.bzl", "aos_config")
+load("//aos/util:config_validator_macro.bzl", "config_validator_test")
+
+config_validator_test(
+ name = "config_validator_test",
+ config = "//y2024_defense:aos_config",
+)
+
+robot_downloader(
+ binaries = [
+ "//aos/network:web_proxy_main",
+ "//aos/events/logging:log_cat",
+ "//aos/events:aos_timing_report_streamer",
+ ],
+ data = [
+ ":aos_config",
+ "//aos/starter:roborio_irq_config.json",
+ "@ctre_phoenix6_api_cpp_athena//:shared_libraries",
+ "@ctre_phoenix6_tools_athena//:shared_libraries",
+ "@ctre_phoenix_api_cpp_athena//:shared_libraries",
+ "@ctre_phoenix_cci_athena//:shared_libraries",
+ ],
+ dirs = [
+ "//y2024_defense/www:www_files",
+ ],
+ start_binaries = [
+ "//aos/events/logging:logger_main",
+ "//aos/network:web_proxy_main",
+ "//aos/starter:irq_affinity",
+ ":joystick_reader",
+ ":wpilib_interface",
+ "//frc971/can_logger",
+ "//aos/network:message_bridge_client",
+ "//aos/network:message_bridge_server",
+ "//y2024_defense/control_loops/drivetrain:drivetrain",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+)
+
+robot_downloader(
+ name = "pi_download",
+ binaries = [
+ "//aos/starter:irq_affinity",
+ "//aos/util:foxglove_websocket",
+ "//aos/events:aos_timing_report_streamer",
+ "//aos/network:web_proxy_main",
+ "//aos/events/logging:log_cat",
+ "//y2024_defense/rockpi:imu_main",
+ "//frc971/image_streamer:image_streamer",
+ ],
+ data = [
+ ":aos_config",
+ "//frc971/rockpi:rockpi_config.json",
+ "//y2024_defense/www:www_files",
+ "@game_pieces_edgetpu_model//file",
+ ],
+ dirs = [
+ "//frc971/image_streamer/www:www_files",
+ ],
+ start_binaries = [
+ "//aos/network:message_bridge_client",
+ "//aos/network:message_bridge_server",
+ "//aos/network:web_proxy_main",
+ "//aos/starter:irq_affinity",
+ "//aos/events/logging:logger_main",
+ ],
+ target_compatible_with = ["//tools/platforms/hardware:raspberry_pi"],
+ target_type = "pi",
+)
+
+aos_config(
+ name = "aos_config",
+ src = "y2024_defense.json",
+ flatbuffers = [
+ "//aos/network:message_bridge_client_fbs",
+ "//aos/network:message_bridge_server_fbs",
+ "//aos/network:timestamp_fbs",
+ "//frc971/input:robot_state_fbs",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":config_imu",
+ ":config_roborio",
+ ],
+)
+
+aos_config(
+ name = "config_imu",
+ src = "y2024_defense_imu.json",
+ flatbuffers = [
+ "//aos/network:message_bridge_client_fbs",
+ "//aos/network:message_bridge_server_fbs",
+ "//aos/network:timestamp_fbs",
+ "//aos/network:remote_message_fbs",
+ "//frc971/control_loops/drivetrain/localization:localizer_output_fbs",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos/events:aos_config",
+ "//frc971/control_loops/drivetrain:aos_config",
+ ],
+)
+
+aos_config(
+ name = "config_roborio",
+ src = "y2024_defense_roborio.json",
+ flatbuffers = [
+ "//frc971:can_configuration_fbs",
+ "//aos/network:remote_message_fbs",
+ "//aos/network:message_bridge_client_fbs",
+ "//aos/network:message_bridge_server_fbs",
+ "//aos/network:timestamp_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_can_position_fbs",
+ "//frc971/can_logger:can_logging_fbs",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ "//aos/events:aos_config",
+ "//frc971/autonomous:aos_config",
+ "//frc971/control_loops/drivetrain:aos_config",
+ "//frc971/input:aos_config",
+ "//frc971/wpilib:aos_config",
+ ],
+)
+
+cc_library(
+ name = "constants",
+ srcs = [
+ "constants.cc",
+ ],
+ hdrs = [
+ "constants.h",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos/mutex",
+ "//aos/network:team_number",
+ "//frc971:constants",
+ "//frc971/control_loops:pose",
+ "//frc971/control_loops:static_zeroing_single_dof_profiled_subsystem",
+ "//frc971/shooter_interpolation:interpolation",
+ "//frc971/zeroing:absolute_encoder",
+ "//frc971/zeroing:pot_and_absolute_encoder",
+ "//y2024_defense/control_loops/drivetrain:polydrivetrain_plants",
+ "@com_github_google_glog//:glog",
+ "@com_google_absl//absl/base",
+ ],
+)
+
+cc_binary(
+ name = "wpilib_interface",
+ srcs = [
+ "wpilib_interface.cc",
+ ],
+ target_compatible_with = ["//tools/platforms/hardware:roborio"],
+ deps = [
+ ":constants",
+ "//aos:init",
+ "//aos:math",
+ "//aos/containers:sized_array",
+ "//aos/events:shm_event_loop",
+ "//aos/logging",
+ "//aos/stl_mutex",
+ "//aos/time",
+ "//aos/util:log_interval",
+ "//aos/util:phased_loop",
+ "//aos/util:wrapping_counter",
+ "//frc971:can_configuration_fbs",
+ "//frc971/autonomous:auto_mode_fbs",
+ "//frc971/control_loops:control_loop",
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_can_position_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_position_fbs",
+ "//frc971/input:robot_state_fbs",
+ "//frc971/queues:gyro_fbs",
+ "//frc971/wpilib:ADIS16448",
+ "//frc971/wpilib:buffered_pcm",
+ "//frc971/wpilib:can_drivetrain_writer",
+ "//frc971/wpilib:can_sensor_reader",
+ "//frc971/wpilib:drivetrain_writer",
+ "//frc971/wpilib:encoder_and_potentiometer",
+ "//frc971/wpilib:falcon",
+ "//frc971/wpilib:interrupt_edge_counting",
+ "//frc971/wpilib:joystick_sender",
+ "//frc971/wpilib:logging_fbs",
+ "//frc971/wpilib:loop_output_handler",
+ "//frc971/wpilib:pdp_fetcher",
+ "//frc971/wpilib:sensor_reader",
+ "//frc971/wpilib:wpilib_interface",
+ "//frc971/wpilib:wpilib_robot_base",
+ "//third_party:phoenix",
+ "//third_party:phoenix6",
+ "//third_party:wpilib",
+ ],
+)
+
+cc_binary(
+ name = "joystick_reader",
+ srcs = [
+ ":joystick_reader.cc",
+ ],
+ deps = [
+ ":constants",
+ "//aos:init",
+ "//aos/actions:action_lib",
+ "//aos/logging",
+ "//frc971/autonomous:auto_fbs",
+ "//frc971/autonomous:base_autonomous_actor",
+ "//frc971/control_loops:profiled_subsystem_fbs",
+ "//frc971/input:action_joystick_input",
+ "//frc971/input:drivetrain_input",
+ "//frc971/input:joystick_input",
+ "//frc971/input:redundant_joystick_data",
+ "//y2024_defense/control_loops/drivetrain:drivetrain_base",
+ ],
+)
+
+py_library(
+ name = "python_init",
+ srcs = ["__init__.py"],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+)
+
+sh_binary(
+ name = "log_web_proxy",
+ srcs = ["log_web_proxy.sh"],
+ data = [
+ ":aos_config",
+ "//aos/network:log_web_proxy_main",
+ "//y2024_defense/www:field_main_bundle.min.js",
+ "//y2024_defense/www:files",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+)
diff --git a/y2024_defense/__init__.py b/y2024_defense/__init__.py
new file mode 100644
index 0000000..e69de29
--- /dev/null
+++ b/y2024_defense/__init__.py
diff --git a/y2024_defense/constants.cc b/y2024_defense/constants.cc
new file mode 100644
index 0000000..fa7a14c
--- /dev/null
+++ b/y2024_defense/constants.cc
@@ -0,0 +1,30 @@
+#include "y2024_defense/constants.h"
+
+#include <cinttypes>
+#include <map>
+
+#if __has_feature(address_sanitizer)
+#include "sanitizer/lsan_interface.h"
+#endif
+
+#include "absl/base/call_once.h"
+#include "glog/logging.h"
+
+#include "aos/mutex/mutex.h"
+#include "aos/network/team_number.h"
+
+namespace y2024_defense {
+namespace constants {
+
+Values MakeValues(uint16_t team) {
+ LOG(INFO) << "creating a Constants for team: " << team;
+
+ Values r;
+
+ return r;
+}
+
+Values MakeValues() { return MakeValues(aos::network::GetTeamNumber()); }
+
+} // namespace constants
+} // namespace y2024_defense
diff --git a/y2024_defense/constants.h b/y2024_defense/constants.h
new file mode 100644
index 0000000..7e359f4
--- /dev/null
+++ b/y2024_defense/constants.h
@@ -0,0 +1,66 @@
+#ifndef Y2023_CONSTANTS_H_
+#define Y2023_CONSTANTS_H_
+
+#include <array>
+#include <cmath>
+#include <cstdint>
+
+#include "frc971/constants.h"
+#include "frc971/control_loops/pose.h"
+#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
+#include "frc971/zeroing/absolute_encoder.h"
+#include "frc971/zeroing/pot_and_absolute_encoder.h"
+#include "y2024_defense/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+
+namespace y2024_defense {
+namespace constants {
+
+constexpr uint16_t kTeamNumber = 9972;
+
+struct Values {
+ static const int kZeroingSampleSize = 200;
+
+ static const int kDrivetrainWriterPriority = 35;
+ static const int kDrivetrainTxPriority = 36;
+ static const int kDrivetrainRxPriority = 36;
+
+ static constexpr double kDrivetrainCyclesPerRevolution() { return 512.0; }
+ static constexpr double kDrivetrainEncoderCountsPerRevolution() {
+ return kDrivetrainCyclesPerRevolution() * 4;
+ }
+ static constexpr double kDrivetrainEncoderRatio() { return 1.0; }
+ static constexpr double kMaxDrivetrainEncoderPulsesPerSecond() {
+ return control_loops::drivetrain::kFreeSpeed / (2.0 * M_PI) *
+ control_loops::drivetrain::kHighOutputRatio /
+ constants::Values::kDrivetrainEncoderRatio() *
+ kDrivetrainEncoderCountsPerRevolution();
+ }
+
+ static constexpr double kDrivetrainSupplyCurrentLimit() { return 35.0; }
+ static constexpr double kDrivetrainStatorCurrentLimit() { return 60.0; }
+
+ static double DrivetrainEncoderToMeters(int32_t in) {
+ return ((static_cast<double>(in) /
+ kDrivetrainEncoderCountsPerRevolution()) *
+ (2.0 * M_PI)) *
+ kDrivetrainEncoderRatio() * control_loops::drivetrain::kWheelRadius;
+ }
+
+ static double DrivetrainCANEncoderToMeters(double rotations) {
+ return (rotations * (2.0 * M_PI)) *
+ control_loops::drivetrain::kHighOutputRatio;
+ }
+};
+
+// Creates and returns a Values instance for the constants.
+// Should be called before realtime because this allocates memory.
+// Only the first call to either of these will be used.
+Values MakeValues(uint16_t team);
+
+// Calls MakeValues with aos::network::GetTeamNumber()
+Values MakeValues();
+
+} // namespace constants
+} // namespace y2024_defense
+
+#endif // Y2023_CONSTANTS_H_
diff --git a/y2024_defense/control_loops/BUILD b/y2024_defense/control_loops/BUILD
new file mode 100644
index 0000000..8dff3b6
--- /dev/null
+++ b/y2024_defense/control_loops/BUILD
@@ -0,0 +1,7 @@
+py_library(
+ name = "python_init",
+ srcs = ["__init__.py"],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = ["//y2024_defense:python_init"],
+)
diff --git a/y2024_defense/control_loops/__init__.py b/y2024_defense/control_loops/__init__.py
new file mode 100644
index 0000000..e69de29
--- /dev/null
+++ b/y2024_defense/control_loops/__init__.py
diff --git a/y2024_defense/control_loops/drivetrain/BUILD b/y2024_defense/control_loops/drivetrain/BUILD
new file mode 100644
index 0000000..664f9a1
--- /dev/null
+++ b/y2024_defense/control_loops/drivetrain/BUILD
@@ -0,0 +1,99 @@
+load("//aos:config.bzl", "aos_config")
+
+genrule(
+ name = "genrule_drivetrain",
+ outs = [
+ "drivetrain_dog_motor_plant.h",
+ "drivetrain_dog_motor_plant.cc",
+ "kalman_drivetrain_motor_plant.h",
+ "kalman_drivetrain_motor_plant.cc",
+ ],
+ cmd = "$(location //y2024_defense/control_loops/python:drivetrain) $(OUTS)",
+ target_compatible_with = ["@platforms//os:linux"],
+ tools = [
+ "//y2024_defense/control_loops/python:drivetrain",
+ ],
+)
+
+genrule(
+ name = "genrule_polydrivetrain",
+ outs = [
+ "polydrivetrain_dog_motor_plant.h",
+ "polydrivetrain_dog_motor_plant.cc",
+ "polydrivetrain_cim_plant.h",
+ "polydrivetrain_cim_plant.cc",
+ "hybrid_velocity_drivetrain.h",
+ "hybrid_velocity_drivetrain.cc",
+ ],
+ cmd = "$(location //y2024_defense/control_loops/python:polydrivetrain) $(OUTS)",
+ target_compatible_with = ["@platforms//os:linux"],
+ tools = [
+ "//y2024_defense/control_loops/python:polydrivetrain",
+ ],
+)
+
+cc_library(
+ name = "polydrivetrain_plants",
+ srcs = [
+ "drivetrain_dog_motor_plant.cc",
+ "hybrid_velocity_drivetrain.cc",
+ "kalman_drivetrain_motor_plant.cc",
+ "polydrivetrain_dog_motor_plant.cc",
+ ],
+ hdrs = [
+ "drivetrain_dog_motor_plant.h",
+ "hybrid_velocity_drivetrain.h",
+ "kalman_drivetrain_motor_plant.h",
+ "polydrivetrain_dog_motor_plant.h",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//frc971/control_loops:hybrid_state_feedback_loop",
+ "//frc971/control_loops:state_feedback_loop",
+ ],
+)
+
+cc_library(
+ name = "drivetrain_base",
+ srcs = [
+ "drivetrain_base.cc",
+ ],
+ hdrs = [
+ "drivetrain_base.h",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":polydrivetrain_plants",
+ "//frc971:shifter_hall_effect",
+ "//frc971/control_loops/drivetrain:drivetrain_config",
+ ],
+)
+
+cc_binary(
+ name = "drivetrain",
+ srcs = [
+ "drivetrain_main.cc",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":drivetrain_base",
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ "//frc971/control_loops/drivetrain:drivetrain_lib",
+ "//frc971/control_loops/drivetrain/localization:puppet_localizer",
+ ],
+)
+
+aos_config(
+ name = "simulation_config",
+ src = "drivetrain_simulation_config.json",
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//frc971/control_loops/drivetrain:simulation_channels",
+ "//y2024_defense:aos_config",
+ ],
+)
diff --git a/y2024_defense/control_loops/drivetrain/drivetrain_base.cc b/y2024_defense/control_loops/drivetrain/drivetrain_base.cc
new file mode 100644
index 0000000..5d14833
--- /dev/null
+++ b/y2024_defense/control_loops/drivetrain/drivetrain_base.cc
@@ -0,0 +1,90 @@
+#include "y2024_defense/control_loops/drivetrain/drivetrain_base.h"
+
+#include <chrono>
+
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "y2024_defense/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "y2024_defense/control_loops/drivetrain/hybrid_velocity_drivetrain.h"
+#include "y2024_defense/control_loops/drivetrain/kalman_drivetrain_motor_plant.h"
+#include "y2024_defense/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
+
+using ::frc971::control_loops::drivetrain::DownEstimatorConfig;
+using ::frc971::control_loops::drivetrain::DrivetrainConfig;
+using ::frc971::control_loops::drivetrain::LineFollowConfig;
+
+namespace chrono = ::std::chrono;
+
+namespace y2024_defense {
+namespace control_loops {
+namespace drivetrain {
+
+using ::frc971::constants::ShifterHallEffect;
+
+const ShifterHallEffect kThreeStateDriveShifter{0.0, 0.0, 0.25, 0.75};
+
+const DrivetrainConfig<double> &GetDrivetrainConfig() {
+ // Yaw of the IMU relative to the robot frame.
+ static constexpr double kImuYaw = 0.0;
+ static DrivetrainConfig<double> kDrivetrainConfig{
+ ::frc971::control_loops::drivetrain::ShifterType::SIMPLE_SHIFTER,
+ ::frc971::control_loops::drivetrain::LoopType::CLOSED_LOOP,
+ ::frc971::control_loops::drivetrain::GyroType::SPARTAN_GYRO,
+ ::frc971::control_loops::drivetrain::IMUType::IMU_FLIPPED_X,
+
+ drivetrain::MakeDrivetrainLoop,
+ drivetrain::MakeVelocityDrivetrainLoop,
+ drivetrain::MakeKFDrivetrainLoop,
+ drivetrain::MakeHybridVelocityDrivetrainLoop,
+
+ chrono::duration_cast<chrono::nanoseconds>(
+ chrono::duration<double>(drivetrain::kDt)),
+ drivetrain::kRobotRadius,
+ drivetrain::kWheelRadius,
+ drivetrain::kV,
+
+ drivetrain::kHighGearRatio,
+ drivetrain::kLowGearRatio,
+ drivetrain::kJ,
+ drivetrain::kMass,
+ kThreeStateDriveShifter,
+ kThreeStateDriveShifter,
+ true /* default_high_gear */,
+ 0 /* down_offset if using constants use
+ constants::GetValues().down_error */
+ ,
+ 0.7 /* wheel_non_linearity */,
+ 1.2 /* quickturn_wheel_multiplier */,
+ 1.2 /* wheel_multiplier */,
+ false /*pistol_grip_shift_enables_line_follow*/,
+ (Eigen::Matrix<double, 3, 3>() << std::cos(kImuYaw), -std::sin(kImuYaw),
+ 0.0, std::sin(kImuYaw), std::cos(kImuYaw), 0.0, 0.0, 0.0, 1.0)
+ .finished(),
+ false /*is_simulated*/,
+ DownEstimatorConfig{.gravity_threshold = 0.015,
+ .do_accel_corrections = 1000},
+ LineFollowConfig{
+ .Q = Eigen::Matrix3d((::Eigen::DiagonalMatrix<double, 3>().diagonal()
+ << 1.0 / ::std::pow(0.1, 2),
+ 1.0 / ::std::pow(1.0, 2),
+ 1.0 / ::std::pow(1.0, 2))
+ .finished()
+ .asDiagonal()),
+ .R = Eigen::Matrix2d((::Eigen::DiagonalMatrix<double, 2>().diagonal()
+ << 10.0 / ::std::pow(12.0, 2),
+ 10.0 / ::std::pow(12.0, 2))
+ .finished()
+ .asDiagonal()),
+ .max_controllable_offset = 0.5},
+ frc971::control_loops::drivetrain::PistolTopButtonUse::kNone,
+ frc971::control_loops::drivetrain::PistolSecondButtonUse::kTurn1,
+ frc971::control_loops::drivetrain::PistolBottomButtonUse::
+ kControlLoopDriving,
+ };
+
+ return kDrivetrainConfig;
+};
+
+} // namespace drivetrain
+} // namespace control_loops
+} // namespace y2024_defense
diff --git a/y2024_defense/control_loops/drivetrain/drivetrain_base.h b/y2024_defense/control_loops/drivetrain/drivetrain_base.h
new file mode 100644
index 0000000..622de4f
--- /dev/null
+++ b/y2024_defense/control_loops/drivetrain/drivetrain_base.h
@@ -0,0 +1,17 @@
+#ifndef Y2023_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_BASE_H_
+#define Y2023_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_BASE_H_
+
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+
+namespace y2024_defense {
+namespace control_loops {
+namespace drivetrain {
+
+const ::frc971::control_loops::drivetrain::DrivetrainConfig<double> &
+GetDrivetrainConfig();
+
+} // namespace drivetrain
+} // namespace control_loops
+} // namespace y2024_defense
+
+#endif // Y2023_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_BASE_H_
diff --git a/y2024_defense/control_loops/drivetrain/drivetrain_main.cc b/y2024_defense/control_loops/drivetrain/drivetrain_main.cc
new file mode 100644
index 0000000..968e312
--- /dev/null
+++ b/y2024_defense/control_loops/drivetrain/drivetrain_main.cc
@@ -0,0 +1,30 @@
+#include <memory>
+
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "frc971/control_loops/drivetrain/drivetrain.h"
+#include "frc971/control_loops/drivetrain/localization/puppet_localizer.h"
+#include "y2024_defense/control_loops/drivetrain/drivetrain_base.h"
+
+using ::frc971::control_loops::drivetrain::DrivetrainLoop;
+
+int main(int argc, char **argv) {
+ aos::InitGoogle(&argc, &argv);
+
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("aos_config.json");
+
+ aos::ShmEventLoop event_loop(&config.message());
+ std::unique_ptr<::frc971::control_loops::drivetrain::PuppetLocalizer>
+ localizer = std::make_unique<
+ ::frc971::control_loops::drivetrain::PuppetLocalizer>(
+ &event_loop,
+ ::y2024_defense::control_loops::drivetrain::GetDrivetrainConfig());
+ std::unique_ptr<DrivetrainLoop> drivetrain = std::make_unique<DrivetrainLoop>(
+ y2024_defense::control_loops::drivetrain::GetDrivetrainConfig(),
+ &event_loop, localizer.get());
+
+ event_loop.Run();
+
+ return 0;
+}
diff --git a/y2024_defense/control_loops/drivetrain/drivetrain_simulation_config.json b/y2024_defense/control_loops/drivetrain/drivetrain_simulation_config.json
new file mode 100644
index 0000000..8251739
--- /dev/null
+++ b/y2024_defense/control_loops/drivetrain/drivetrain_simulation_config.json
@@ -0,0 +1,6 @@
+{
+ "imports": [
+ "../../y2024_defense.json",
+ "../../../frc971/control_loops/drivetrain/drivetrain_simulation_channels.json"
+ ]
+}
diff --git a/y2024_defense/control_loops/python/BUILD b/y2024_defense/control_loops/python/BUILD
new file mode 100644
index 0000000..eec5e9f
--- /dev/null
+++ b/y2024_defense/control_loops/python/BUILD
@@ -0,0 +1,57 @@
+package(default_visibility = ["//y2024_defense:__subpackages__"])
+
+py_binary(
+ name = "drivetrain",
+ srcs = [
+ "drivetrain.py",
+ ],
+ legacy_create_init = False,
+ target_compatible_with = ["@platforms//cpu:x86_64"],
+ deps = [
+ ":python_init",
+ "//frc971/control_loops/python:drivetrain",
+ "@pip//glog",
+ "@pip//python_gflags",
+ ],
+)
+
+py_binary(
+ name = "polydrivetrain",
+ srcs = [
+ "drivetrain.py",
+ "polydrivetrain.py",
+ ],
+ legacy_create_init = False,
+ target_compatible_with = ["@platforms//cpu:x86_64"],
+ deps = [
+ ":python_init",
+ "//frc971/control_loops/python:polydrivetrain",
+ "@pip//glog",
+ "@pip//python_gflags",
+ ],
+)
+
+py_library(
+ name = "polydrivetrain_lib",
+ srcs = [
+ "drivetrain.py",
+ "polydrivetrain.py",
+ ],
+ target_compatible_with = ["@platforms//cpu:x86_64"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//frc971/control_loops/python:controls",
+ "//frc971/control_loops/python:drivetrain",
+ "//frc971/control_loops/python:polydrivetrain",
+ "@pip//glog",
+ "@pip//python_gflags",
+ ],
+)
+
+py_library(
+ name = "python_init",
+ srcs = ["__init__.py"],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = ["//y2024_defense/control_loops:python_init"],
+)
diff --git a/y2024_defense/control_loops/python/__init__.py b/y2024_defense/control_loops/python/__init__.py
new file mode 100644
index 0000000..e69de29
--- /dev/null
+++ b/y2024_defense/control_loops/python/__init__.py
diff --git a/y2024_defense/control_loops/python/drivetrain.py b/y2024_defense/control_loops/python/drivetrain.py
new file mode 100644
index 0000000..438eb24
--- /dev/null
+++ b/y2024_defense/control_loops/python/drivetrain.py
@@ -0,0 +1,48 @@
+#!/usr/bin/python3
+
+from __future__ import print_function
+from frc971.control_loops.python import drivetrain
+from frc971.control_loops.python import control_loop
+import sys
+
+import gflags
+import glog
+
+FLAGS = gflags.FLAGS
+
+gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+
+kDrivetrain = drivetrain.DrivetrainParams(
+ J=6.5,
+ mass=68.0,
+ # TODO(austin): Measure radius a bit better.
+ robot_radius=0.39,
+ wheel_radius=2.5 * 0.0254,
+ motor_type=control_loop.Falcon(),
+ num_motors=3,
+ G=(14.0 / 52.0) * (26.0 / 58.0),
+ q_pos=0.24,
+ q_vel=2.5,
+ efficiency=0.92,
+ has_imu=False,
+ force=True,
+ kf_q_voltage=1.0,
+ controller_poles=[0.82, 0.82])
+
+
+def main(argv):
+ argv = FLAGS(argv)
+ glog.init()
+
+ if FLAGS.plot:
+ drivetrain.PlotDrivetrainMotions(kDrivetrain)
+ elif len(argv) != 5:
+ print("Expected .h file name and .cc file name")
+ else:
+ # Write the generated constants out to a file.
+ drivetrain.WriteDrivetrain(argv[1:3], argv[3:5], 'y2024_defense',
+ kDrivetrain)
+
+
+if __name__ == '__main__':
+ sys.exit(main(sys.argv))
diff --git a/y2024_defense/control_loops/python/polydrivetrain.py b/y2024_defense/control_loops/python/polydrivetrain.py
new file mode 100644
index 0000000..301c7b5
--- /dev/null
+++ b/y2024_defense/control_loops/python/polydrivetrain.py
@@ -0,0 +1,34 @@
+#!/usr/bin/python3
+
+import sys
+from y2024_defense.control_loops.python import drivetrain
+from frc971.control_loops.python import polydrivetrain
+
+import gflags
+import glog
+
+__author__ = 'Austin Schuh (austin.linux@gmail.com)'
+
+FLAGS = gflags.FLAGS
+
+try:
+ gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+except gflags.DuplicateFlagError:
+ pass
+
+
+def main(argv):
+ if FLAGS.plot:
+ polydrivetrain.PlotPolyDrivetrainMotions(drivetrain.kDrivetrain)
+ elif len(argv) != 7:
+ glog.fatal('Expected .h file name and .cc file name')
+ else:
+ polydrivetrain.WritePolyDrivetrain(argv[1:3], argv[3:5], argv[5:7],
+ 'y2024_defense',
+ drivetrain.kDrivetrain)
+
+
+if __name__ == '__main__':
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2024_defense/joystick_reader.cc b/y2024_defense/joystick_reader.cc
new file mode 100644
index 0000000..8c3115b
--- /dev/null
+++ b/y2024_defense/joystick_reader.cc
@@ -0,0 +1,65 @@
+#include <unistd.h>
+
+#include <cmath>
+#include <cstdio>
+#include <cstring>
+
+#include "aos/actions/actions.h"
+#include "aos/init.h"
+#include "aos/logging/logging.h"
+#include "aos/network/team_number.h"
+#include "aos/util/log_interval.h"
+#include "frc971/autonomous/base_autonomous_actor.h"
+#include "frc971/control_loops/drivetrain/localizer_generated.h"
+#include "frc971/control_loops/profiled_subsystem_generated.h"
+#include "frc971/input/action_joystick_input.h"
+#include "frc971/input/driver_station_data.h"
+#include "frc971/input/drivetrain_input.h"
+#include "frc971/input/joystick_input.h"
+#include "frc971/input/redundant_joystick_data.h"
+#include "frc971/zeroing/wrap.h"
+#include "y2024_defense/constants.h"
+#include "y2024_defense/control_loops/drivetrain/drivetrain_base.h"
+
+using Side = frc971::control_loops::drivetrain::RobotSide;
+
+namespace y2024_defense {
+namespace input {
+namespace joysticks {
+
+class Reader : public ::frc971::input::ActionJoystickInput {
+ public:
+ Reader(::aos::EventLoop *event_loop)
+ : ::frc971::input::ActionJoystickInput(
+ event_loop,
+ ::y2024_defense::control_loops::drivetrain::GetDrivetrainConfig(),
+ ::frc971::input::DrivetrainInputReader::InputType::kPistol,
+ {.use_redundant_joysticks = true}) {}
+
+ void AutoEnded() override { AOS_LOG(INFO, "Auto ended.\n"); }
+
+ bool has_scored_ = false;
+
+ void HandleTeleop(
+ const ::frc971::input::driver_station::Data &data) override {
+ (void)data;
+ }
+};
+
+} // namespace joysticks
+} // namespace input
+} // namespace y2024_defense
+
+int main(int argc, char **argv) {
+ ::aos::InitGoogle(&argc, &argv);
+
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("aos_config.json");
+
+ ::aos::ShmEventLoop event_loop(&config.message());
+ ::y2024_defense::input::joysticks::Reader reader(&event_loop);
+
+ event_loop.Run();
+
+ return 0;
+}
diff --git a/y2024_defense/log_web_proxy.sh b/y2024_defense/log_web_proxy.sh
new file mode 100755
index 0000000..714d491
--- /dev/null
+++ b/y2024_defense/log_web_proxy.sh
@@ -0,0 +1 @@
+./aos/network/log_web_proxy_main --data_dir=y2024_defense/www $@
diff --git a/y2024_defense/rockpi/BUILD b/y2024_defense/rockpi/BUILD
new file mode 100644
index 0000000..756a694
--- /dev/null
+++ b/y2024_defense/rockpi/BUILD
@@ -0,0 +1,12 @@
+cc_binary(
+ name = "imu_main",
+ srcs = ["imu_main.cc"],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ "//frc971/imu_reader:imu",
+ "//y2024_defense:constants",
+ ],
+)
diff --git a/y2024_defense/rockpi/imu_main.cc b/y2024_defense/rockpi/imu_main.cc
new file mode 100644
index 0000000..8ab51fe
--- /dev/null
+++ b/y2024_defense/rockpi/imu_main.cc
@@ -0,0 +1,26 @@
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "aos/realtime.h"
+#include "frc971/imu_reader/imu.h"
+#include "y2024_defense/constants.h"
+
+DEFINE_string(config, "aos_config.json", "Path to the config file to use.");
+
+int main(int argc, char *argv[]) {
+ aos::InitGoogle(&argc, &argv);
+
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig(FLAGS_config);
+
+ aos::ShmEventLoop event_loop(&config.message());
+ frc971::imu::Imu imu(
+ &event_loop,
+ y2024_defense::constants::Values::DrivetrainEncoderToMeters(1));
+
+ event_loop.SetRuntimeAffinity(aos::MakeCpusetFromCpus({0}));
+ event_loop.SetRuntimeRealtimePriority(55);
+
+ event_loop.Run();
+
+ return 0;
+}
diff --git a/y2024_defense/wpilib_interface.cc b/y2024_defense/wpilib_interface.cc
new file mode 100644
index 0000000..7cecb21
--- /dev/null
+++ b/y2024_defense/wpilib_interface.cc
@@ -0,0 +1,381 @@
+#include <unistd.h>
+
+#include <array>
+#include <chrono>
+#include <cinttypes>
+#include <cmath>
+#include <cstdio>
+#include <cstring>
+#include <functional>
+#include <memory>
+#include <mutex>
+#include <thread>
+
+#include "ctre/phoenix/CANifier.h"
+
+#include "frc971/wpilib/ahal/AnalogInput.h"
+#include "frc971/wpilib/ahal/Counter.h"
+#include "frc971/wpilib/ahal/DigitalGlitchFilter.h"
+#include "frc971/wpilib/ahal/DriverStation.h"
+#include "frc971/wpilib/ahal/Encoder.h"
+#include "frc971/wpilib/ahal/Servo.h"
+#include "frc971/wpilib/ahal/TalonFX.h"
+#include "frc971/wpilib/ahal/VictorSP.h"
+#undef ERROR
+
+#include "ctre/phoenix/cci/Diagnostics_CCI.h"
+#include "ctre/phoenix6/TalonFX.hpp"
+
+#include "aos/commonmath.h"
+#include "aos/containers/sized_array.h"
+#include "aos/events/event_loop.h"
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "aos/logging/logging.h"
+#include "aos/realtime.h"
+#include "aos/time/time.h"
+#include "aos/util/log_interval.h"
+#include "aos/util/phased_loop.h"
+#include "aos/util/wrapping_counter.h"
+#include "frc971/autonomous/auto_mode_generated.h"
+#include "frc971/can_configuration_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_can_position_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
+#include "frc971/input/robot_state_generated.h"
+#include "frc971/queues/gyro_generated.h"
+#include "frc971/wpilib/ADIS16448.h"
+#include "frc971/wpilib/buffered_pcm.h"
+#include "frc971/wpilib/buffered_solenoid.h"
+#include "frc971/wpilib/can_drivetrain_writer.h"
+#include "frc971/wpilib/can_sensor_reader.h"
+#include "frc971/wpilib/dma.h"
+#include "frc971/wpilib/drivetrain_writer.h"
+#include "frc971/wpilib/encoder_and_potentiometer.h"
+#include "frc971/wpilib/falcon.h"
+#include "frc971/wpilib/joystick_sender.h"
+#include "frc971/wpilib/logging_generated.h"
+#include "frc971/wpilib/loop_output_handler.h"
+#include "frc971/wpilib/pdp_fetcher.h"
+#include "frc971/wpilib/sensor_reader.h"
+#include "frc971/wpilib/wpilib_robot_base.h"
+#include "y2024_defense/constants.h"
+
+DEFINE_bool(ctre_diag_server, false,
+ "If true, enable the diagnostics server for interacting with "
+ "devices on the CAN bus using Phoenix Tuner");
+
+using ::aos::monotonic_clock;
+using ::frc971::CANConfiguration;
+using ::y2024_defense::constants::Values;
+
+using frc971::control_loops::drivetrain::CANPosition;
+using frc971::wpilib::Falcon;
+
+using std::make_unique;
+
+namespace y2024_defense {
+namespace wpilib {
+namespace {
+
+constexpr double kMaxBringupPower = 12.0;
+
+double drivetrain_velocity_translate(double in) {
+ return (((1.0 / in) / Values::kDrivetrainCyclesPerRevolution()) *
+ (2.0 * M_PI)) *
+ Values::kDrivetrainEncoderRatio() *
+ control_loops::drivetrain::kWheelRadius;
+}
+
+constexpr double kMaxFastEncoderPulsesPerSecond = std::max({
+ Values::kMaxDrivetrainEncoderPulsesPerSecond(),
+});
+static_assert(kMaxFastEncoderPulsesPerSecond <= 1300000,
+ "fast encoders are too fast");
+
+} // namespace
+
+// Class to send position messages with sensor readings to our loops.
+class SensorReader : public ::frc971::wpilib::SensorReader {
+ public:
+ SensorReader(::aos::ShmEventLoop *event_loop,
+ std::shared_ptr<const Values> values)
+ : ::frc971::wpilib::SensorReader(event_loop),
+ values_(std::move(values)),
+ auto_mode_sender_(
+ event_loop->MakeSender<::frc971::autonomous::AutonomousMode>(
+ "/autonomous")),
+ drivetrain_position_sender_(
+ event_loop
+ ->MakeSender<::frc971::control_loops::drivetrain::Position>(
+ "/drivetrain")),
+ gyro_sender_(event_loop->MakeSender<::frc971::sensors::GyroReading>(
+ "/drivetrain")) {
+ // Set to filter out anything shorter than 1/4 of the minimum pulse width
+ // we should ever see.
+ UpdateFastEncoderFilterHz(kMaxFastEncoderPulsesPerSecond);
+ event_loop->SetRuntimeAffinity(aos::MakeCpusetFromCpus({0}));
+ }
+
+ void Start() override { AddToDMA(&imu_yaw_rate_reader_); }
+
+ // Auto mode switches.
+ void set_autonomous_mode(int i, ::std::unique_ptr<frc::DigitalInput> sensor) {
+ autonomous_modes_.at(i) = ::std::move(sensor);
+ }
+
+ void set_yaw_rate_input(::std::unique_ptr<frc::DigitalInput> sensor) {
+ imu_yaw_rate_input_ = ::std::move(sensor);
+ imu_yaw_rate_reader_.set_input(imu_yaw_rate_input_.get());
+ }
+
+ void RunIteration() override {
+ superstructure_reading_->Set(true);
+ {
+ {
+ auto builder = drivetrain_position_sender_.MakeBuilder();
+ frc971::control_loops::drivetrain::Position::Builder
+ drivetrain_builder =
+ builder
+ .MakeBuilder<frc971::control_loops::drivetrain::Position>();
+ drivetrain_builder.add_left_encoder(
+ constants::Values::DrivetrainEncoderToMeters(
+ drivetrain_left_encoder_->GetRaw()));
+ drivetrain_builder.add_left_speed(drivetrain_velocity_translate(
+ drivetrain_left_encoder_->GetPeriod()));
+
+ drivetrain_builder.add_right_encoder(
+ -constants::Values::DrivetrainEncoderToMeters(
+ drivetrain_right_encoder_->GetRaw()));
+ drivetrain_builder.add_right_speed(-drivetrain_velocity_translate(
+ drivetrain_right_encoder_->GetPeriod()));
+
+ builder.CheckOk(builder.Send(drivetrain_builder.Finish()));
+ }
+
+ {
+ auto builder = gyro_sender_.MakeBuilder();
+ ::frc971::sensors::GyroReading::Builder gyro_reading_builder =
+ builder.MakeBuilder<::frc971::sensors::GyroReading>();
+ // +/- 2000 deg / sec
+ constexpr double kMaxVelocity = 4000; // degrees / second
+ constexpr double kVelocityRadiansPerSecond =
+ kMaxVelocity / 360 * (2.0 * M_PI);
+
+ // Only part of the full range is used to prevent being 100% on or off.
+ constexpr double kScaledRangeLow = 0.1;
+ constexpr double kScaledRangeHigh = 0.9;
+
+ constexpr double kPWMFrequencyHz = 200;
+ double velocity_duty_cycle =
+ imu_yaw_rate_reader_.last_width() * kPWMFrequencyHz;
+
+ constexpr double kDutyCycleScale =
+ 1 / (kScaledRangeHigh - kScaledRangeLow);
+ // scale from 0.1 - 0.9 to 0 - 1
+ double rescaled_velocity_duty_cycle =
+ (velocity_duty_cycle - kScaledRangeLow) * kDutyCycleScale;
+
+ if (!std::isnan(rescaled_velocity_duty_cycle)) {
+ gyro_reading_builder.add_velocity(
+ (rescaled_velocity_duty_cycle - 0.5) * kVelocityRadiansPerSecond);
+ }
+ builder.CheckOk(builder.Send(gyro_reading_builder.Finish()));
+ }
+
+ {
+ auto builder = auto_mode_sender_.MakeBuilder();
+
+ uint32_t mode = 0;
+ for (size_t i = 0; i < autonomous_modes_.size(); ++i) {
+ if (autonomous_modes_[i] && autonomous_modes_[i]->Get()) {
+ mode |= 1 << i;
+ }
+ }
+
+ auto auto_mode_builder =
+ builder.MakeBuilder<frc971::autonomous::AutonomousMode>();
+
+ auto_mode_builder.add_mode(mode);
+
+ builder.CheckOk(builder.Send(auto_mode_builder.Finish()));
+ }
+ }
+ }
+
+ std::shared_ptr<frc::DigitalOutput> superstructure_reading_;
+
+ void set_superstructure_reading(
+ std::shared_ptr<frc::DigitalOutput> superstructure_reading) {
+ superstructure_reading_ = superstructure_reading;
+ }
+
+ private:
+ std::shared_ptr<const Values> values_;
+
+ aos::Sender<frc971::autonomous::AutonomousMode> auto_mode_sender_;
+ aos::Sender<frc971::control_loops::drivetrain::Position>
+ drivetrain_position_sender_;
+ ::aos::Sender<::frc971::sensors::GyroReading> gyro_sender_;
+
+ std::array<std::unique_ptr<frc::DigitalInput>, 2> autonomous_modes_;
+
+ std::unique_ptr<frc::DigitalInput> imu_yaw_rate_input_;
+
+ frc971::wpilib::DMAPulseWidthReader imu_yaw_rate_reader_;
+};
+
+class WPILibRobot : public ::frc971::wpilib::WPILibRobotBase {
+ public:
+ ::std::unique_ptr<frc::Encoder> make_encoder(int index) {
+ return make_unique<frc::Encoder>(10 + index * 2, 11 + index * 2, false,
+ frc::Encoder::k4X);
+ }
+
+ void Run() override {
+ std::shared_ptr<const Values> values =
+ std::make_shared<const Values>(constants::MakeValues());
+
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("aos_config.json");
+
+ // Thread 1.
+ ::aos::ShmEventLoop joystick_sender_event_loop(&config.message());
+ ::frc971::wpilib::JoystickSender joystick_sender(
+ &joystick_sender_event_loop);
+ AddLoop(&joystick_sender_event_loop);
+
+ // Thread 2.
+ ::aos::ShmEventLoop pdp_fetcher_event_loop(&config.message());
+ ::frc971::wpilib::PDPFetcher pdp_fetcher(&pdp_fetcher_event_loop);
+ AddLoop(&pdp_fetcher_event_loop);
+
+ // Thread 3.
+ ::aos::ShmEventLoop sensor_reader_event_loop(&config.message());
+ SensorReader sensor_reader(&sensor_reader_event_loop, values);
+ std::shared_ptr<frc::DigitalOutput> superstructure_reading =
+ make_unique<frc::DigitalOutput>(25);
+
+ sensor_reader.set_pwm_trigger(true);
+ sensor_reader.set_drivetrain_left_encoder(make_encoder(1));
+ sensor_reader.set_drivetrain_right_encoder(make_encoder(0));
+ sensor_reader.set_superstructure_reading(superstructure_reading);
+ sensor_reader.set_yaw_rate_input(make_unique<frc::DigitalInput>(0));
+
+ AddLoop(&sensor_reader_event_loop);
+
+ // Thread 4.
+ std::vector<ctre::phoenix6::BaseStatusSignal *> signals_registry;
+
+ std::shared_ptr<Falcon> right_front = std::make_shared<Falcon>(
+ 1, true, "Drivetrain Bus", &signals_registry,
+ constants::Values::kDrivetrainStatorCurrentLimit(),
+ constants::Values::kDrivetrainSupplyCurrentLimit());
+ std::shared_ptr<Falcon> right_back = std::make_shared<Falcon>(
+ 2, true, "Drivetrain Bus", &signals_registry,
+ constants::Values::kDrivetrainStatorCurrentLimit(),
+ constants::Values::kDrivetrainSupplyCurrentLimit());
+ std::shared_ptr<Falcon> right_under = std::make_shared<Falcon>(
+ 3, true, "Drivetrain Bus", &signals_registry,
+ constants::Values::kDrivetrainStatorCurrentLimit(),
+ constants::Values::kDrivetrainSupplyCurrentLimit());
+ std::shared_ptr<Falcon> left_front = std::make_shared<Falcon>(
+ 4, false, "Drivetrain Bus", &signals_registry,
+ constants::Values::kDrivetrainStatorCurrentLimit(),
+ constants::Values::kDrivetrainSupplyCurrentLimit());
+ std::shared_ptr<Falcon> left_back = std::make_shared<Falcon>(
+ 5, false, "Drivetrain Bus", &signals_registry,
+ constants::Values::kDrivetrainStatorCurrentLimit(),
+ constants::Values::kDrivetrainSupplyCurrentLimit());
+ std::shared_ptr<Falcon> left_under = std::make_shared<Falcon>(
+ 6, false, "Drivetrain Bus", &signals_registry,
+ constants::Values::kDrivetrainStatorCurrentLimit(),
+ constants::Values::kDrivetrainSupplyCurrentLimit());
+
+ // Setting up CAN.
+ if (!FLAGS_ctre_diag_server) {
+ c_Phoenix_Diagnostics_SetSecondsToStart(-1);
+ c_Phoenix_Diagnostics_Dispose();
+ }
+
+ // Creating list of falcons for CANSensorReader
+ std::vector<std::shared_ptr<Falcon>> falcons;
+ for (auto falcon : {right_front, right_back, right_under, left_front,
+ left_back, left_under}) {
+ falcons.push_back(falcon);
+ }
+
+ ctre::phoenix::platform::can::CANComm_SetRxSchedPriority(
+ constants::Values::kDrivetrainRxPriority, true, "Drivetrain Bus");
+ ctre::phoenix::platform::can::CANComm_SetTxSchedPriority(
+ constants::Values::kDrivetrainTxPriority, true, "Drivetrain Bus");
+
+ ::aos::ShmEventLoop can_sensor_reader_event_loop(&config.message());
+ can_sensor_reader_event_loop.set_name("CANSensorReader");
+
+ aos::Sender<CANPosition> can_position_sender =
+ can_sensor_reader_event_loop.MakeSender<CANPosition>("/drivetrain");
+
+ frc971::wpilib::CANSensorReader can_sensor_reader(
+ &can_sensor_reader_event_loop, std::move(signals_registry), falcons,
+ [falcons, &can_position_sender](ctre::phoenix::StatusCode status) {
+ auto builder = can_position_sender.MakeBuilder();
+ aos::SizedArray<flatbuffers::Offset<frc971::control_loops::CANFalcon>,
+ 6>
+ flatbuffer_falcons;
+
+ for (auto falcon : falcons) {
+ falcon->SerializePosition(
+ builder.fbb(), control_loops::drivetrain::kHighOutputRatio);
+ std::optional<flatbuffers::Offset<frc971::control_loops::CANFalcon>>
+ falcon_offset = falcon->TakeOffset();
+
+ CHECK(falcon_offset.has_value());
+
+ flatbuffer_falcons.push_back(falcon_offset.value());
+ }
+
+ auto falcons_list =
+ builder.fbb()
+ ->CreateVector<
+ flatbuffers::Offset<frc971::control_loops::CANFalcon>>(
+ flatbuffer_falcons);
+
+ frc971::control_loops::drivetrain::CANPosition::Builder
+ can_position_builder = builder.MakeBuilder<
+ frc971::control_loops::drivetrain::CANPosition>();
+
+ can_position_builder.add_falcons(falcons_list);
+ can_position_builder.add_timestamp(falcons.front()->GetTimestamp());
+ can_position_builder.add_status(static_cast<int>(status));
+
+ builder.CheckOk(builder.Send(can_position_builder.Finish()));
+ });
+
+ AddLoop(&can_sensor_reader_event_loop);
+
+ // Thread 5.
+ ::aos::ShmEventLoop can_drivetrain_writer_event_loop(&config.message());
+ can_drivetrain_writer_event_loop.set_name("CANDrivetrainWriter");
+
+ frc971::wpilib::CANDrivetrainWriter can_drivetrain_writer(
+ &can_drivetrain_writer_event_loop);
+
+ can_drivetrain_writer.set_falcons({right_front, right_back, right_under},
+ {left_front, left_back, left_under});
+
+ can_drivetrain_writer_event_loop.MakeWatcher(
+ "/roborio", [&can_drivetrain_writer](
+ const frc971::CANConfiguration &configuration) {
+ can_drivetrain_writer.HandleCANConfiguration(configuration);
+ });
+
+ AddLoop(&can_drivetrain_writer_event_loop);
+
+ RunLoops();
+ }
+};
+
+} // namespace wpilib
+} // namespace y2024_defense
+
+AOS_ROBOT_CLASS(::y2024_defense::wpilib::WPILibRobot);
diff --git a/y2024_defense/www/BUILD b/y2024_defense/www/BUILD
new file mode 100644
index 0000000..7af68fe
--- /dev/null
+++ b/y2024_defense/www/BUILD
@@ -0,0 +1,63 @@
+load("//tools/build_rules:js.bzl", "rollup_bundle", "ts_project")
+load("//frc971/downloader:downloader.bzl", "aos_downloader_dir")
+
+filegroup(
+ name = "files",
+ srcs = glob([
+ "**/*.html",
+ "**/*.css",
+ "**/*.png",
+ ]) + ["2023.png"],
+ visibility = ["//visibility:public"],
+)
+
+# TODO(max): This needs to be changed once the season starts
+genrule(
+ name = "2023_field_png",
+ srcs = ["//third_party/y2023/field:pictures"],
+ outs = ["2023.png"],
+ cmd = "cp third_party/y2023/field/2023.png $@",
+)
+
+ts_project(
+ name = "field_main",
+ srcs = [
+ "constants.ts",
+ "field_handler.ts",
+ "field_main.ts",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ "//aos/network:connect_ts_fbs",
+ "//aos/network:message_bridge_client_ts_fbs",
+ "//aos/network:message_bridge_server_ts_fbs",
+ "//aos/network:web_proxy_ts_fbs",
+ "//aos/network/www:proxy",
+ "//frc971/control_loops:control_loops_ts_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_status_ts_fbs",
+ "//frc971/control_loops/drivetrain/localization:localizer_output_ts_fbs",
+ "@com_github_google_flatbuffers//ts:flatbuffers_ts",
+ ],
+)
+
+rollup_bundle(
+ name = "field_main_bundle",
+ entry_point = "field_main.ts",
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//y2024_defense:__subpackages__"],
+ deps = [
+ ":field_main",
+ ],
+)
+
+aos_downloader_dir(
+ name = "www_files",
+ srcs = [
+ ":field_main_bundle.min.js",
+ ":files",
+ "//frc971/analysis:plot_index_bundle.min.js",
+ ],
+ dir = "www",
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+)
diff --git a/y2024_defense/www/constants.ts b/y2024_defense/www/constants.ts
new file mode 100644
index 0000000..d6ecfaf
--- /dev/null
+++ b/y2024_defense/www/constants.ts
@@ -0,0 +1,8 @@
+// Conversion constants to meters
+export const IN_TO_M = 0.0254;
+export const FT_TO_M = 0.3048;
+// Dimensions of the field in meters
+// Numbers are slightly hand-tuned to match the PNG that we are using.
+export const FIELD_WIDTH = 26 * FT_TO_M + 7.25 * IN_TO_M;
+export const FIELD_LENGTH = 54 * FT_TO_M + 5.25 * IN_TO_M;
+
diff --git a/y2024_defense/www/field.html b/y2024_defense/www/field.html
new file mode 100644
index 0000000..d086af7
--- /dev/null
+++ b/y2024_defense/www/field.html
@@ -0,0 +1,41 @@
+<html>
+ <head>
+ <script src="field_main_bundle.min.js" defer></script>
+ <link rel="stylesheet" href="styles.css">
+ </head>
+ <body>
+ <div id="field"> </div>
+ <div id="legend"> </div>
+ <div id="readouts">
+ <table>
+ <tr>
+ <th colspan="2">Robot State</th>
+ </tr>
+ <tr>
+ <td>X</td>
+ <td id="x"> NA </td>
+ </tr>
+ <tr>
+ <td>Y</td>
+ <td id="y"> NA </td>
+ </tr>
+ <tr>
+ <td>Theta</td>
+ <td id="theta"> NA </td>
+ </tr>
+ </table>
+ <h3>Zeroing Faults:</h3>
+ <p id="zeroing_faults"> NA </p>
+ </div>
+ <div id="middle_readouts">
+ <div id="message_bridge_status">
+ <div>
+ <div>Node</div>
+ <div>Client</div>
+ <div>Server</div>
+ </div>
+ </div>
+ </div>
+ </body>
+</html>
+
diff --git a/y2024_defense/www/field_handler.ts b/y2024_defense/www/field_handler.ts
new file mode 100644
index 0000000..8010b10
--- /dev/null
+++ b/y2024_defense/www/field_handler.ts
@@ -0,0 +1,261 @@
+import {ByteBuffer} from 'flatbuffers'
+import {ClientStatistics} from '../../aos/network/message_bridge_client_generated'
+import {ServerStatistics, State as ConnectionState} from '../../aos/network/message_bridge_server_generated'
+import {Connection} from '../../aos/network/www/proxy'
+import {ZeroingError} from '../../frc971/control_loops/control_loops_generated'
+import {Status as DrivetrainStatus} from '../../frc971/control_loops/drivetrain/drivetrain_status_generated'
+import {LocalizerOutput} from '../../frc971/control_loops/drivetrain/localization/localizer_output_generated'
+
+import {FIELD_LENGTH, FIELD_WIDTH, FT_TO_M, IN_TO_M} from './constants';
+
+// (0,0) is field center, +X is toward red DS
+const FIELD_SIDE_Y = FIELD_WIDTH / 2;
+const FIELD_EDGE_X = FIELD_LENGTH / 2;
+
+const ROBOT_WIDTH = 25 * IN_TO_M;
+const ROBOT_LENGTH = 32 * IN_TO_M;
+
+const PI_COLORS = ['#ff00ff', '#ffff00', '#00ffff', '#ffa500'];
+const PIS = ['pi1', 'pi2', 'pi3', 'pi4'];
+
+export class FieldHandler {
+ private canvas = document.createElement('canvas');
+ private localizerOutput: LocalizerOutput|null = null;
+ private drivetrainStatus: DrivetrainStatus|null = null;
+
+ private x: HTMLElement = (document.getElementById('x') as HTMLElement);
+ private y: HTMLElement = (document.getElementById('y') as HTMLElement);
+ private theta: HTMLElement =
+ (document.getElementById('theta') as HTMLElement);
+ // HTML elements for rejection reasons for individual pis. Indices
+ // corresponding to RejectionReason enum values will be for those reasons. The
+ // final row will account for images rejected by the aprilrobotics detector
+ // instead of the localizer.
+ private messageBridgeDiv: HTMLElement =
+ (document.getElementById('message_bridge_status') as HTMLElement);
+ private clientStatuses = new Map<string, HTMLElement>();
+ private serverStatuses = new Map<string, HTMLElement>();
+ private fieldImage: HTMLImageElement = new Image();
+
+ constructor(private readonly connection: Connection) {
+ (document.getElementById('field') as HTMLElement).appendChild(this.canvas);
+
+ this.fieldImage.src = '2023.png';
+
+ this.connection.addConfigHandler(() => {
+ // Visualization message is reliable so that we can see *all* the vision
+ // matches.
+ this.connection.addHandler(
+ '/drivetrain', 'frc971.control_loops.drivetrain.Status', (data) => {
+ this.handleDrivetrainStatus(data);
+ });
+ this.connection.addHandler(
+ '/localizer', 'frc971.controls.LocalizerOutput', (data) => {
+ this.handleLocalizerOutput(data);
+ });
+ this.connection.addHandler(
+ '/aos', 'aos.message_bridge.ServerStatistics',
+ (data) => {this.handleServerStatistics(data)});
+ this.connection.addHandler(
+ '/aos', 'aos.message_bridge.ClientStatistics',
+ (data) => {this.handleClientStatistics(data)});
+ });
+ }
+
+ private handleLocalizerOutput(data: Uint8Array): void {
+ const fbBuffer = new ByteBuffer(data);
+ this.localizerOutput = LocalizerOutput.getRootAsLocalizerOutput(fbBuffer);
+ }
+
+ private handleDrivetrainStatus(data: Uint8Array): void {
+ const fbBuffer = new ByteBuffer(data);
+ this.drivetrainStatus = DrivetrainStatus.getRootAsStatus(fbBuffer);
+ }
+
+ private populateNodeConnections(nodeName: string): void {
+ const row = document.createElement('div');
+ this.messageBridgeDiv.appendChild(row);
+ const nodeDiv = document.createElement('div');
+ nodeDiv.innerHTML = nodeName;
+ row.appendChild(nodeDiv);
+ const clientDiv = document.createElement('div');
+ clientDiv.innerHTML = 'N/A';
+ row.appendChild(clientDiv);
+ const serverDiv = document.createElement('div');
+ serverDiv.innerHTML = 'N/A';
+ row.appendChild(serverDiv);
+ this.serverStatuses.set(nodeName, serverDiv);
+ this.clientStatuses.set(nodeName, clientDiv);
+ }
+
+ private setCurrentNodeState(element: HTMLElement, state: ConnectionState):
+ void {
+ if (state === ConnectionState.CONNECTED) {
+ element.innerHTML = ConnectionState[state];
+ element.classList.remove('faulted');
+ element.classList.add('connected');
+ } else {
+ element.innerHTML = ConnectionState[state];
+ element.classList.remove('connected');
+ element.classList.add('faulted');
+ }
+ }
+
+ private handleServerStatistics(data: Uint8Array): void {
+ const fbBuffer = new ByteBuffer(data);
+ const serverStatistics =
+ ServerStatistics.getRootAsServerStatistics(fbBuffer);
+
+ for (let ii = 0; ii < serverStatistics.connectionsLength(); ++ii) {
+ const connection = serverStatistics.connections(ii);
+ const nodeName = connection.node().name();
+ if (!this.serverStatuses.has(nodeName)) {
+ this.populateNodeConnections(nodeName);
+ }
+ this.setCurrentNodeState(
+ this.serverStatuses.get(nodeName), connection.state());
+ }
+ }
+
+ private handleClientStatistics(data: Uint8Array): void {
+ const fbBuffer = new ByteBuffer(data);
+ const clientStatistics =
+ ClientStatistics.getRootAsClientStatistics(fbBuffer);
+
+ for (let ii = 0; ii < clientStatistics.connectionsLength(); ++ii) {
+ const connection = clientStatistics.connections(ii);
+ const nodeName = connection.node().name();
+ if (!this.clientStatuses.has(nodeName)) {
+ this.populateNodeConnections(nodeName);
+ }
+ this.setCurrentNodeState(
+ this.clientStatuses.get(nodeName), connection.state());
+ }
+ }
+
+ setZeroing(div: HTMLElement): void {
+ div.innerHTML = 'zeroing';
+ div.classList.remove('faulted');
+ div.classList.add('zeroing');
+ div.classList.remove('near');
+ }
+
+ setValue(div: HTMLElement, val: number): void {
+ div.innerHTML = val.toFixed(4);
+ div.classList.remove('faulted');
+ div.classList.remove('zeroing');
+ div.classList.remove('near');
+ }
+
+ drawField(): void {
+ const ctx = this.canvas.getContext('2d');
+ ctx.save();
+ ctx.scale(1.0, -1.0);
+ ctx.drawImage(
+ this.fieldImage, 0, 0, this.fieldImage.width, this.fieldImage.height,
+ -FIELD_EDGE_X, -FIELD_SIDE_Y, FIELD_LENGTH, FIELD_WIDTH);
+ ctx.restore();
+ }
+
+ drawCamera(x: number, y: number, theta: number, color: string = 'blue'):
+ void {
+ const ctx = this.canvas.getContext('2d');
+ ctx.save();
+ ctx.translate(x, y);
+ ctx.rotate(theta);
+ ctx.strokeStyle = color;
+ ctx.beginPath();
+ ctx.moveTo(0.5, 0.5);
+ ctx.lineTo(0, 0);
+ ctx.lineTo(0.5, -0.5);
+ ctx.stroke();
+ ctx.beginPath();
+ ctx.arc(0, 0, 0.25, -Math.PI / 4, Math.PI / 4);
+ ctx.stroke();
+ ctx.restore();
+ }
+
+ drawRobot(
+ x: number, y: number, theta: number, color: string = 'blue',
+ dashed: boolean = false): void {
+ const ctx = this.canvas.getContext('2d');
+ ctx.save();
+ ctx.translate(x, y);
+ ctx.rotate(theta);
+ ctx.strokeStyle = color;
+ ctx.lineWidth = ROBOT_WIDTH / 10.0;
+ if (dashed) {
+ ctx.setLineDash([0.05, 0.05]);
+ } else {
+ // Empty array = solid line.
+ ctx.setLineDash([]);
+ }
+ ctx.rect(-ROBOT_LENGTH / 2, -ROBOT_WIDTH / 2, ROBOT_LENGTH, ROBOT_WIDTH);
+ ctx.stroke();
+
+ // Draw line indicating which direction is forwards on the robot.
+ ctx.beginPath();
+ ctx.moveTo(0, 0);
+ ctx.lineTo(ROBOT_LENGTH / 2.0, 0);
+ ctx.stroke();
+
+ ctx.restore();
+ }
+
+ draw(): void {
+ this.reset();
+ this.drawField();
+
+ // Draw the matches with debugging information from the localizer.
+ const now = Date.now() / 1000.0;
+
+ if (this.drivetrainStatus && this.drivetrainStatus.trajectoryLogging()) {
+ this.drawRobot(
+ this.drivetrainStatus.trajectoryLogging().x(),
+ this.drivetrainStatus.trajectoryLogging().y(),
+ this.drivetrainStatus.trajectoryLogging().theta(), '#000000FF',
+ false);
+ }
+
+ if (this.localizerOutput) {
+ if (!this.localizerOutput.zeroed()) {
+ this.setZeroing(this.x);
+ this.setZeroing(this.y);
+ this.setZeroing(this.theta);
+ } else {
+ this.setValue(this.x, this.localizerOutput.x());
+ this.setValue(this.y, this.localizerOutput.y());
+ this.setValue(this.theta, this.localizerOutput.theta());
+ }
+
+ this.drawRobot(
+ this.localizerOutput.x(), this.localizerOutput.y(),
+ this.localizerOutput.theta());
+ }
+
+
+ window.requestAnimationFrame(() => this.draw());
+ }
+
+ reset(): void {
+ const ctx = this.canvas.getContext('2d');
+ ctx.setTransform(1, 0, 0, 1, 0, 0);
+ const size = window.innerHeight * 0.9;
+ ctx.canvas.height = size;
+ const width = size / 2 + 20;
+ ctx.canvas.width = width;
+ ctx.clearRect(0, 0, size, width);
+
+ // Translate to center of display.
+ ctx.translate(width / 2, size / 2);
+ // Coordinate system is:
+ // x -> forward.
+ // y -> to the left.
+ ctx.rotate(-Math.PI / 2);
+ ctx.scale(1, -1);
+
+ const M_TO_PX = (size - 10) / FIELD_LENGTH;
+ ctx.scale(M_TO_PX, M_TO_PX);
+ ctx.lineWidth = 1 / M_TO_PX;
+ }
+}
diff --git a/y2024_defense/www/field_main.ts b/y2024_defense/www/field_main.ts
new file mode 100644
index 0000000..d71a45e
--- /dev/null
+++ b/y2024_defense/www/field_main.ts
@@ -0,0 +1,12 @@
+import {Connection} from '../../aos/network/www/proxy';
+
+import {FieldHandler} from './field_handler';
+
+const conn = new Connection();
+
+conn.connect();
+
+const fieldHandler = new FieldHandler(conn);
+
+fieldHandler.draw();
+
diff --git a/y2024_defense/www/index.html b/y2024_defense/www/index.html
new file mode 100644
index 0000000..e4e185e
--- /dev/null
+++ b/y2024_defense/www/index.html
@@ -0,0 +1,6 @@
+<html>
+ <body>
+ <a href="field.html">Field Visualization</a><br>
+ <a href="plotter.html">Plots</a>
+ </body>
+</html>
diff --git a/y2024_defense/www/plotter.html b/y2024_defense/www/plotter.html
new file mode 100644
index 0000000..629ceaa
--- /dev/null
+++ b/y2024_defense/www/plotter.html
@@ -0,0 +1,7 @@
+<html>
+ <head>
+ <script src="plot_index_bundle.min.js" defer></script>
+ </head>
+ <body>
+ </body>
+</html>
diff --git a/y2024_defense/www/styles.css b/y2024_defense/www/styles.css
new file mode 100644
index 0000000..c2c44d2
--- /dev/null
+++ b/y2024_defense/www/styles.css
@@ -0,0 +1,74 @@
+.channel {
+ display: flex;
+ border-bottom: 1px solid;
+ font-size: 24px;
+}
+#field {
+ display: inline-block
+}
+
+#readouts,
+#middle_readouts
+{
+ display: inline-block;
+ vertical-align: top;
+ float: right;
+}
+
+
+#legend {
+ display: inline-block;
+}
+
+table, th, td {
+ border: 1px solid black;
+ border-collapse: collapse;
+ padding: 5px;
+ margin: 10px;
+}
+
+th, td {
+ text-align: right;
+ width: 70px;
+}
+
+td:first-child {
+ width: 150px;
+}
+
+.connected, .near {
+ background-color: LightGreen;
+ border-radius: 10px;
+}
+
+.zeroing {
+ background-color: yellow;
+ border-radius: 10px;
+}
+
+.faulted {
+ background-color: red;
+ border-radius: 10px;
+}
+
+#vision_readouts > div {
+ display: table-row;
+ padding: 5px;
+}
+
+#vision_readouts > div > div {
+ display: table-cell;
+ padding: 5px;
+ text-align: right;
+}
+
+#message_bridge_status > div {
+ display: table-row;
+ padding: 5px;
+}
+
+#message_bridge_status > div > div {
+ display: table-cell;
+ padding: 5px;
+ text-align: right;
+}
diff --git a/y2024_defense/y2024_defense.json b/y2024_defense/y2024_defense.json
new file mode 100644
index 0000000..03e7373
--- /dev/null
+++ b/y2024_defense/y2024_defense.json
@@ -0,0 +1,18 @@
+{
+ "channel_storage_duration": 10000000000,
+ "maps": [
+ {
+ "match": {
+ "name": "/aos",
+ "type": "aos.RobotState"
+ },
+ "rename": {
+ "name": "/roborio/aos"
+ }
+ }
+ ],
+ "imports": [
+ "y2024_defense_roborio.json",
+ "y2024_defense_imu.json",
+ ]
+}
diff --git a/y2024_defense/y2024_defense_imu.json b/y2024_defense/y2024_defense_imu.json
new file mode 100644
index 0000000..1d64398
--- /dev/null
+++ b/y2024_defense/y2024_defense_imu.json
@@ -0,0 +1,280 @@
+{
+ "channels": [
+ {
+ "name": "/imu/aos",
+ "type": "aos.JoystickState",
+ "source_node": "imu",
+ "frequency": 100,
+ "logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": [
+ "roborio"
+ ]
+ },
+ {
+ "name": "/imu/aos",
+ "type": "aos.timing.Report",
+ "source_node": "imu",
+ "frequency": 50,
+ "num_senders": 20,
+ "max_size": 4096
+ },
+ {
+ "name": "/imu/aos",
+ "type": "aos.logging.LogMessageFbs",
+ "source_node": "imu",
+ "frequency": 200,
+ "num_senders": 20
+ },
+ {
+ "name": "/imu/aos",
+ "type": "aos.starter.Status",
+ "source_node": "imu",
+ "frequency": 50,
+ "num_senders": 20,
+ "max_size": 2048
+ },
+ {
+ "name": "/imu/aos",
+ "type": "aos.starter.StarterRpc",
+ "source_node": "imu",
+ "frequency": 10,
+ "num_senders": 2
+ },
+ {
+ "name": "/imu/aos",
+ "type": "aos.message_bridge.ServerStatistics",
+ "source_node": "imu",
+ "max_size": 2048,
+ "frequency": 10,
+ "num_senders": 2
+ },
+ {
+ "name": "/imu/aos",
+ "type": "aos.message_bridge.ClientStatistics",
+ "source_node": "imu",
+ "frequency": 20,
+ "num_senders": 2
+ },
+ {
+ "name": "/imu/aos",
+ "type": "aos.logging.DynamicLogCommand",
+ "source_node": "imu",
+ "frequency": 10,
+ "num_senders": 2
+ },
+ {
+ "name": "/imu/aos",
+ "type": "aos.message_bridge.Timestamp",
+ "source_node": "imu",
+ "frequency": 15,
+ "num_senders": 2,
+ "logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": [
+ "roborio",
+ ],
+ "max_size": 400,
+ "destination_nodes": [
+ {
+ "name": "roborio",
+ "priority": 1,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "imu"
+ ],
+ "time_to_live": 5000000
+ },
+ ]
+ },
+ {
+ "name": "/imu/aos/remote_timestamps/roborio/imu/aos/aos-message_bridge-Timestamp",
+ "type": "aos.message_bridge.RemoteMessage",
+ "frequency": 20,
+ "source_node": "imu",
+ "max_size": 208
+ },
+ {
+ "name": "/roborio/aos",
+ "type": "aos.starter.StarterRpc",
+ "source_node": "roborio",
+ "logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": [
+ "imu"
+ ],
+ "destination_nodes": [
+ {
+ "name": "imu",
+ "priority": 5,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "roborio"
+ ],
+ "time_to_live": 5000000
+ }
+ ]
+ },
+ {
+ "name": "/roborio/aos/remote_timestamps/imu/roborio/aos/aos-starter-StarterRpc",
+ "type": "aos.message_bridge.RemoteMessage",
+ "source_node": "roborio",
+ "logger": "NOT_LOGGED",
+ "frequency": 20,
+ "num_senders": 2,
+ "max_size": 200
+ },
+ {
+ "name": "/localizer",
+ "type": "frc971.IMUValuesBatch",
+ "source_node": "imu",
+ "frequency": 2200,
+ "max_size": 1600,
+ "num_senders": 2
+ },
+ {
+ "name": "/localizer",
+ "type": "frc971.controls.LocalizerOutput",
+ "source_node": "imu",
+ "frequency": 52,
+ "logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": [
+ "roborio"
+ ],
+ "destination_nodes": [
+ {
+ "name": "roborio",
+ "priority": 5,
+ "time_to_live": 5000000,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "imu"
+ ]
+ }
+ ]
+ },
+ {
+ "name": "/imu/aos/remote_timestamps/roborio/localizer/frc971-controls-LocalizerOutput",
+ "type": "aos.message_bridge.RemoteMessage",
+ "source_node": "imu",
+ "logger": "NOT_LOGGED",
+ "frequency": 52,
+ "num_senders": 2,
+ "max_size": 200
+ },
+ ],
+ "applications": [
+ {
+ "name": "message_bridge_client",
+ "nodes": [
+ "imu"
+ ]
+ },
+ {
+ "name": "localizer",
+ "executable_name": "localizer_main",
+ "user": "pi",
+ "nodes": [
+ "imu"
+ ]
+ },
+ {
+ "name": "imu",
+ "executable_name": "imu_main",
+ "user": "pi",
+ "nodes": [
+ "imu"
+ ]
+ },
+ {
+ "name": "joystick_republish",
+ "executable_name": "joystick_republish",
+ "user": "pi",
+ "nodes": [
+ "imu"
+ ]
+ },
+ {
+ "name": "message_bridge_server",
+ "executable_name": "message_bridge_server",
+ "user": "pi",
+ "nodes": [
+ "imu"
+ ]
+ },
+ {
+ "name": "localizer_logger",
+ "executable_name": "logger_main",
+ "args": [
+ "--logging_folder",
+ "",
+ "--snappy_compress",
+ "--rotate_every", "30.0"
+ ],
+ "user": "pi",
+ "nodes": [
+ "imu"
+ ]
+ },
+ {
+ "name": "web_proxy",
+ "executable_name": "web_proxy_main",
+ "args": [
+ "--min_ice_port=5800",
+ "--max_ice_port=5810"
+ ],
+ "user": "pi",
+ "nodes": [
+ "imu"
+ ]
+ },
+ {
+ "name": "foxglove_websocket",
+ "user": "pi",
+ "nodes": [
+ "imu"
+ ]
+ },
+ {
+ "name": "constants_sender",
+ "autorestart": false,
+ "user": "pi",
+ "nodes": [
+ "imu"
+ ]
+ }
+ ],
+ "maps": [
+ {
+ "match": {
+ "name": "/constants*",
+ "source_node": "imu"
+ },
+ "rename": {
+ "name": "/imu/constants"
+ }
+ },
+ {
+ "match": {
+ "name": "/aos*",
+ "source_node": "imu"
+ },
+ "rename": {
+ "name": "/imu/aos"
+ }
+ }
+ ],
+ "nodes": [
+ {
+ "name": "imu",
+ "hostname": "pi6",
+ "hostnames": [
+ "pi-971-6",
+ "pi-7971-6",
+ "pi-8971-6",
+ "pi-9971-6"
+ ],
+ "port": 9971
+ },
+ {
+ "name": "roborio"
+ },
+ ]
+}
diff --git a/y2024_defense/y2024_defense_roborio.json b/y2024_defense/y2024_defense_roborio.json
new file mode 100644
index 0000000..f397de3
--- /dev/null
+++ b/y2024_defense/y2024_defense_roborio.json
@@ -0,0 +1,385 @@
+{
+ "channels": [
+ {
+ "name": "/roborio/aos",
+ "type": "aos.JoystickState",
+ "source_node": "roborio",
+ "frequency": 100,
+ "logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": [
+ "imu"
+ ],
+ "destination_nodes": [
+ {
+ "name": "imu",
+ "priority": 5,
+ "time_to_live": 50000000
+ }
+ ]
+ },
+ {
+ "name": "/roborio/aos",
+ "type": "aos.RobotState",
+ "source_node": "roborio",
+ "frequency": 250
+ },
+ {
+ "name": "/roborio/aos",
+ "type": "aos.timing.Report",
+ "source_node": "roborio",
+ "frequency": 50,
+ "num_senders": 20,
+ "max_size": 8192
+ },
+ {
+ "name": "/roborio/aos",
+ "type": "aos.logging.LogMessageFbs",
+ "source_node": "roborio",
+ "frequency": 500,
+ "max_size": 1000,
+ "num_senders": 20
+ },
+ {
+ "name": "/roborio/aos",
+ "type": "aos.starter.Status",
+ "source_node": "roborio",
+ "frequency": 50,
+ "num_senders": 20,
+ "max_size": 2000
+ },
+ {
+ "name": "/roborio/aos",
+ "type": "aos.starter.StarterRpc",
+ "source_node": "roborio",
+ "frequency": 10,
+ "max_size": 400,
+ "num_senders": 2
+ },
+ {
+ "name": "/roborio/aos",
+ "type": "aos.message_bridge.ServerStatistics",
+ "source_node": "roborio",
+ "max_size": 2048,
+ "frequency": 10,
+ "num_senders": 2
+ },
+ {
+ "name": "/roborio/aos",
+ "type": "aos.message_bridge.ClientStatistics",
+ "source_node": "roborio",
+ "frequency": 20,
+ "max_size": 2000,
+ "num_senders": 2
+ },
+ {
+ "name": "/roborio/aos",
+ "type": "aos.logging.DynamicLogCommand",
+ "source_node": "roborio",
+ "frequency": 10,
+ "num_senders": 2
+ },
+ {
+ "name": "/roborio/aos/remote_timestamps/imu/roborio/aos/aos-message_bridge-Timestamp",
+ "type": "aos.message_bridge.RemoteMessage",
+ "frequency": 20,
+ "source_node": "roborio",
+ "max_size": 208
+ },
+ {
+ "name": "/roborio/aos",
+ "type": "aos.message_bridge.Timestamp",
+ "source_node": "roborio",
+ "frequency": 15,
+ "num_senders": 2,
+ "max_size": 512,
+ "logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": [
+ "imu"
+ ],
+ "destination_nodes": [
+ {
+ "name": "imu",
+ "priority": 1,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "roborio"
+ ],
+ "time_to_live": 5000000
+ }
+ ]
+ },
+ {
+ "name": "/can",
+ "type": "frc971.can_logger.CanFrame",
+ "source_node": "roborio",
+ "frequency": 6000,
+ "num_senders": 2,
+ "max_size": 200
+ },
+ {
+ "name": "/drivetrain",
+ "type": "frc971.control_loops.drivetrain.CANPosition",
+ "source_node": "roborio",
+ "frequency": 220,
+ "num_senders": 2,
+ "max_size": 400
+ },
+ {
+ "name": "/drivetrain",
+ "type": "frc971.sensors.GyroReading",
+ "source_node": "roborio",
+ "frequency": 250,
+ "num_senders": 2
+ },
+ {
+ "name": "/drivetrain",
+ "type": "frc971.sensors.Uid",
+ "source_node": "roborio",
+ "frequency": 250,
+ "num_senders": 2
+ },
+ {
+ "name": "/drivetrain",
+ "type": "frc971.control_loops.drivetrain.fb.Trajectory",
+ "source_node": "roborio",
+ "max_size": 600000,
+ "frequency": 10,
+ "logger": "NOT_LOGGED",
+ "num_senders": 2,
+ "read_method": "PIN",
+ "num_readers": 10
+ },
+ {
+ "name": "/drivetrain",
+ "type": "frc971.control_loops.drivetrain.SplineGoal",
+ "source_node": "roborio",
+ "frequency": 10
+ },
+ {
+ "name": "/drivetrain",
+ "type": "frc971.control_loops.drivetrain.Goal",
+ "source_node": "roborio",
+ "max_size": 224,
+ "frequency": 250
+ },
+ {
+ "name": "/drivetrain",
+ "type": "frc971.control_loops.drivetrain.Position",
+ "source_node": "roborio",
+ "frequency": 400,
+ "max_size": 112,
+ "num_senders": 2
+ },
+ {
+ "name": "/drivetrain",
+ "type": "frc971.control_loops.drivetrain.Output",
+ "source_node": "roborio",
+ "frequency": 400,
+ "max_size": 80,
+ "num_senders": 2,
+ "logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": [
+ "imu"
+ ],
+ "destination_nodes": [
+ {
+ "name": "imu",
+ "priority": 5,
+ "time_to_live": 5000000
+ }
+ ]
+ },
+ {
+ "name": "/drivetrain",
+ "type": "frc971.control_loops.drivetrain.Status",
+ "source_node": "roborio",
+ "frequency": 400,
+ "max_size": 1616,
+ "num_senders": 2
+ },
+ {
+ "name": "/drivetrain",
+ "type": "frc971.control_loops.drivetrain.LocalizerControl",
+ "source_node": "roborio",
+ "frequency": 250,
+ "max_size": 96,
+ "logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": [
+ "imu"
+ ],
+ "destination_nodes": [
+ {
+ "name": "imu",
+ "priority": 5,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "roborio"
+ ],
+ "time_to_live": 0
+ }
+ ]
+ },
+ {
+ "name": "/roborio/aos/remote_timestamps/imu/drivetrain/frc971-control_loops-drivetrain-LocalizerControl",
+ "type": "aos.message_bridge.RemoteMessage",
+ "source_node": "roborio",
+ "logger": "NOT_LOGGED",
+ "frequency": 400,
+ "num_senders": 2,
+ "max_size": 200
+ },
+ {
+ "name": "/autonomous",
+ "type": "aos.common.actions.Status",
+ "source_node": "roborio"
+ },
+ {
+ "name": "/autonomous",
+ "type": "frc971.autonomous.Goal",
+ "source_node": "roborio"
+ },
+ {
+ "name": "/autonomous",
+ "type": "frc971.autonomous.AutonomousMode",
+ "source_node": "roborio",
+ "frequency": 250
+ },
+ {
+ "name": "/roborio/aos",
+ "type": "frc971.PDPValues",
+ "source_node": "roborio",
+ "frequency": 55,
+ "max_size": 368
+ },
+ {
+ "name": "/roborio/aos",
+ "type": "frc971.wpilib.PneumaticsToLog",
+ "source_node": "roborio",
+ "frequency": 50
+ },
+ {
+ "name": "/roborio",
+ "type": "frc971.CANConfiguration",
+ "source_node": "roborio",
+ "frequency": 2
+ },
+ ],
+ "applications": [
+ {
+ "name": "drivetrain",
+ "executable_name": "drivetrain",
+ "nodes": [
+ "roborio"
+ ]
+ },
+ {
+ "name": "roborio_irq_affinity",
+ "executable_name": "irq_affinity",
+ "args": [
+ "--irq_config=/home/admin/bin/roborio_irq_config.json"
+ ],
+ "nodes": [
+ "roborio"
+ ]
+ },
+ {
+ "name": "joystick_reader",
+ "executable_name": "joystick_reader",
+ "args": [
+ "--nodie_on_malloc"
+ ],
+ "nodes": [
+ "roborio"
+ ]
+ },
+ {
+ "name": "wpilib_interface",
+ "executable_name": "wpilib_interface",
+ "args": [
+ "--nodie_on_malloc"
+ ],
+ "nodes": [
+ "roborio"
+ ]
+ },
+ {
+ "name": "roborio_web_proxy",
+ "executable_name": "web_proxy_main",
+ "args": [
+ "--min_ice_port=5800",
+ "--max_ice_port=5810"
+ ],
+ "nodes": [
+ "roborio"
+ ]
+ },
+ {
+ "name": "roborio_message_bridge_client",
+ "executable_name": "message_bridge_client",
+ "args": [
+ "--rt_priority=16",
+ "--sinit_max_init_timeout=5000"
+ ],
+ "nodes": [
+ "roborio"
+ ]
+ },
+ {
+ "name": "roborio_message_bridge_server",
+ "executable_name": "message_bridge_server",
+ "args": [
+ "--rt_priority=16"
+ ],
+ "nodes": [
+ "roborio"
+ ]
+ },
+ {
+ "name": "logger",
+ "executable_name": "logger_main",
+ "args": [
+ "--snappy_compress",
+ "--logging_folder=/home/admin/logs/",
+ "--rotate_every", "30.0"
+ ],
+ "nodes": [
+ "roborio"
+ ]
+ },
+ {
+ "name": "can_logger",
+ "executable_name": "can_logger",
+ "nodes": [
+ "roborio"
+ ]
+ }
+ ],
+ "maps": [
+ {
+ "match": {
+ "name": "/aos*",
+ "source_node": "roborio"
+ },
+ "rename": {
+ "name": "/roborio/aos"
+ }
+ }
+ ],
+ "nodes": [
+ {
+ "name": "roborio",
+ "hostname": "roborio",
+ "hostnames": [
+ "roboRIO-971-FRC",
+ "roboRIO-6971-FRC",
+ "roboRIO-7971-FRC",
+ "roboRIO-8971-FRC",
+ "roboRIO-9972-FRC"
+ ],
+ "port": 9972
+ },
+ {
+ "name": "imu"
+ }
+ ]
+}