Merge "Add shooter superstructure code"
diff --git a/WORKSPACE b/WORKSPACE
index 40f7cc3..d252f41 100644
--- a/WORKSPACE
+++ b/WORKSPACE
@@ -1546,8 +1546,8 @@
# This contains the *compiled* foxglove studio. This can be reproduced by:
# 1. Cloning https://github.com/foxglove/studio
-# 2. Building the code (yarn web:build:prod)
-# 3. tar'ing the web/.webpack folder
+# 2. Building the code (yarn install; yarn web:build:prod)
+# 3. tar'ing the web/.webpack folder (e.g., tar czvf foxglove-1456f4a4cb6f4c6c7e50e020ba9918dba9e04b96.tar.gz --directory=web/.webpack/ .)
# These files can be hosted locally to provide an offline foxglove server.
# Foxglove may be served on any port and may be nested at a subpath
# (e.g., at hostname:8000/foxglove behind a proxy).
@@ -1559,9 +1559,9 @@
srcs = glob(["**"]),
visibility = ["//visibility:public"],
)""",
- sha256 = "68513024efb60dcdfc9b130d3e0466d194a8599fbd85f8e99d01c148d03e5887",
+ sha256 = "d02f4ca629e6dcf2b65557a0353871ce0025e70715214de4e6ec7e9f862de420",
url =
- "https://software.frc971.org/Build-Dependencies/foxglove-9857d637e90dfeecd63ad47fa760046791f8d43c.tar.gz",
+ "https://software.frc971.org/Build-Dependencies/foxglove-1456f4a4cb6f4c6c7e50e020ba9918dba9e04b96.tar.gz",
)
#
diff --git a/aos/util/error_counter.h b/aos/util/error_counter.h
index 3e69a71..1e04571 100644
--- a/aos/util/error_counter.h
+++ b/aos/util/error_counter.h
@@ -41,6 +41,16 @@
return offset;
}
+ template <typename Static>
+ static void InitializeStaticFbs(Static *builder) {
+ CHECK(builder->reserve(kNumErrors));
+ for (size_t ii = 0; ii < kNumErrors; ++ii) {
+ auto element = CHECK_NOTNULL(builder->emplace_back());
+ element->set_error(static_cast<Error>(ii));
+ element->set_count(0);
+ }
+ }
+
void set_mutable_vector(
flatbuffers::Vector<flatbuffers::Offset<Count>> *vector) {
vector_ = vector;
@@ -91,6 +101,14 @@
return offset;
}
+ template <typename Static>
+ void PopulateCountsStaticFbs(Static *builder) const {
+ ErrorCounter<Error, Count>::InitializeStaticFbs(builder);
+ for (size_t ii = 0; ii < kNumErrors; ++ii) {
+ builder->at(ii).set_count(error_counts_.at(ii));
+ }
+ }
+
void IncrementError(Error error) {
DCHECK_LT(static_cast<size_t>(error), error_counts_.size());
error_counts_.at(static_cast<size_t>(error))++;
diff --git a/documentation/tutorials/rio-gdb.md b/documentation/tutorials/rio-gdb.md
new file mode 100644
index 0000000..eb7bc16
--- /dev/null
+++ b/documentation/tutorials/rio-gdb.md
@@ -0,0 +1,29 @@
+# Roborio GDB
+
+The roborio has a gdb-server command so we need to set the rio as our server and connect to it with some host device.
+
+## Steps
+
+### Rio
+This command will start the gdb-server program on the rio.
+```bash
+gdbserver localhost:8898 program
+```
+
+### Host
+```bash
+# this is the gdb-multiarch package on debian. Its needed because your host machine
+# likely isn't using the architecture the rio uses.
+gdb-multiarch
+# This connects us using an extended remote to the roborio, which lets us use the run command.
+target extended-remote ip:8898
+# Here we specify the name of program from the rio side
+set remote exec-file program
+# This makes it so we don't stop when getting realtime signals
+handle SIG33 pass nostop noprint
+# From here on you can use gdb like you normally would
+# So we can run our program with some args
+r args
+# To exit the gdb-server program on the rio side we need to run this command.
+monitor exit
+```
diff --git a/frc971/control_loops/aiming/aiming.cc b/frc971/control_loops/aiming/aiming.cc
index 4cf9255..572ed1c 100644
--- a/frc971/control_loops/aiming/aiming.cc
+++ b/frc971/control_loops/aiming/aiming.cc
@@ -54,6 +54,8 @@
} // namespace
TurretGoal AimerGoal(const ShotConfig &config, const RobotState &state) {
+ CHECK(config.ball_speed_over_ground > 0.0)
+ << ": Ball speed must be positive.";
TurretGoal result;
// This code manages compensating the goal turret heading for the robot's
// current velocity, to allow for shooting on-the-fly.
diff --git a/frc971/control_loops/aiming/aiming_test.cc b/frc971/control_loops/aiming/aiming_test.cc
index 4ec7eb9..a4f7bf2 100644
--- a/frc971/control_loops/aiming/aiming_test.cc
+++ b/frc971/control_loops/aiming/aiming_test.cc
@@ -158,4 +158,17 @@
EXPECT_FLOAT_EQ(0.0, goal.velocity);
}
+// Tests that we fail if the ballspeed is zero. Can't hit anything if the ball
+// isn't moving.
+TEST(AimerDeathTest, ZeroBallSpeed) {
+ const Pose target({0.0, 0.0, 0.0}, 0.0);
+ Pose robot_pose({1.0, 1.0, 1.0}, 0.0);
+ const constants::Range range{-2.36, 2.36, -2.16, 2.16};
+ const double kBallSpeed = 0.0;
+ EXPECT_DEATH(AimerGoal(ShotConfig{target, ShotMode::kShootOnTheFly, range,
+ kBallSpeed, 0.0, 0.0},
+ RobotState{robot_pose, {0.0, 0.0}, 0.0, 0.0}),
+ "Ball speed must be positive.");
+}
+
} // namespace frc971::control_loops::aiming::testing
diff --git a/frc971/imu_fdcan/BUILD b/frc971/imu_fdcan/BUILD
index deb6a7e..c8a6cd7 100644
--- a/frc971/imu_fdcan/BUILD
+++ b/frc971/imu_fdcan/BUILD
@@ -1,3 +1,4 @@
+load("//aos:config.bzl", "aos_config")
load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
static_flatbuffer(
@@ -5,3 +6,125 @@
srcs = ["dual_imu.fbs"],
visibility = ["//visibility:public"],
)
+
+static_flatbuffer(
+ name = "can_translator_status_fbs",
+ srcs = ["can_translator_status.fbs"],
+ visibility = ["//visibility:public"],
+)
+
+static_flatbuffer(
+ name = "dual_imu_blender_status_fbs",
+ srcs = ["dual_imu_blender_status.fbs"],
+ visibility = ["//visibility:public"],
+)
+
+cc_binary(
+ name = "can_translator",
+ srcs = ["can_translator_main.cc"],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":can_translator_lib",
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ ],
+)
+
+cc_library(
+ name = "can_translator_lib",
+ srcs = [
+ "can_translator_lib.cc",
+ ],
+ hdrs = [
+ "can_translator_lib.h",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":can_translator_status_fbs",
+ ":dual_imu_fbs",
+ "//aos/events:event_loop",
+ "//frc971/can_logger:can_logging_fbs",
+ ],
+)
+
+cc_binary(
+ name = "dual_imu_blender",
+ srcs = ["dual_imu_blender_main.cc"],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":dual_imu_blender_lib",
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ ],
+)
+
+cc_library(
+ name = "dual_imu_blender_lib",
+ srcs = [
+ "dual_imu_blender_lib.cc",
+ ],
+ hdrs = [
+ "dual_imu_blender_lib.h",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":dual_imu_blender_status_fbs",
+ ":dual_imu_fbs",
+ "//aos/events:event_loop",
+ "//frc971/can_logger:can_logging_fbs",
+ "//frc971/wpilib:imu_batch_fbs",
+ ],
+)
+
+cc_test(
+ name = "can_translator_lib_test",
+ srcs = [
+ "can_translator_lib_test.cc",
+ ],
+ data = [
+ ":dual_imu_test_config",
+ ],
+ deps = [
+ ":can_translator_lib",
+ ":can_translator_status_fbs",
+ ":dual_imu_fbs",
+ "//aos/events:simulated_event_loop",
+ "//aos/testing:googletest",
+ "//frc971/can_logger:can_logging_fbs",
+ "@com_github_google_glog//:glog",
+ ],
+)
+
+cc_test(
+ name = "dual_imu_blender_lib_test",
+ srcs = [
+ "dual_imu_blender_lib_test.cc",
+ ],
+ data = [
+ ":dual_imu_test_config",
+ ],
+ deps = [
+ ":dual_imu_blender_lib",
+ ":dual_imu_blender_status_fbs",
+ ":dual_imu_fbs",
+ "//aos/events:simulated_event_loop",
+ "//aos/testing:googletest",
+ "//frc971/can_logger:can_logging_fbs",
+ "@com_github_google_glog//:glog",
+ ],
+)
+
+aos_config(
+ name = "dual_imu_test_config",
+ src = "dual_imu_test_config_source.json",
+ flatbuffers = [
+ "//aos/logging:log_message_fbs",
+ ":dual_imu_fbs",
+ ":can_translator_status_fbs",
+ "//frc971/can_logger:can_logging_fbs",
+ ":dual_imu_blender_status_fbs",
+ "//frc971/wpilib:imu_batch_fbs",
+ "//aos/events:event_loop_fbs",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+)
diff --git a/frc971/imu_fdcan/can_translator_lib.cc b/frc971/imu_fdcan/can_translator_lib.cc
new file mode 100644
index 0000000..b5e5576
--- /dev/null
+++ b/frc971/imu_fdcan/can_translator_lib.cc
@@ -0,0 +1,125 @@
+#include "frc971/imu_fdcan/can_translator_lib.h"
+
+using frc971::imu_fdcan::CANTranslator;
+
+constexpr std::size_t kCanFrameSize = 64;
+constexpr int kImuCanId = 1;
+
+CANTranslator::CANTranslator(aos::EventLoop *event_loop,
+ std::string_view canframe_channel)
+ : dual_imu_sender_(
+ event_loop->MakeSender<frc971::imu::DualImuStatic>("/imu")),
+ can_translator_status_sender_(
+ event_loop->MakeSender<frc971::imu::CanTranslatorStatusStatic>(
+ "/imu")) {
+ // TODO(max): Update this with a proper priority
+ event_loop->SetRuntimeRealtimePriority(15);
+
+ event_loop->MakeWatcher(
+ canframe_channel, [this](const frc971::can_logger::CanFrame &can_frame) {
+ if (can_frame.data()->size() / sizeof(uint8_t) != 64) {
+ invalid_packet_count_++;
+ }
+
+ if (can_frame.can_id() != kImuCanId) {
+ invalid_can_id_count_++;
+ return;
+ }
+
+ if (can_frame.data()->size() / sizeof(uint8_t) == 64) {
+ valid_packet_count_++;
+ HandleFrame(&can_frame);
+ }
+ });
+
+ event_loop->AddPhasedLoop(
+ [this](int) {
+ aos::Sender<frc971::imu::CanTranslatorStatusStatic>::StaticBuilder
+ status_builder = can_translator_status_sender_.MakeStaticBuilder();
+
+ status_builder->set_valid_packet_count(valid_packet_count_);
+ status_builder->set_invalid_packet_count(invalid_packet_count_);
+ status_builder->set_invalid_can_id_count(invalid_can_id_count_);
+
+ status_builder.CheckOk(status_builder.Send());
+ },
+ std::chrono::milliseconds(100));
+}
+
+// Gets the data from the span and iterates it to the next section of bytes.
+template <typename T>
+T GetAndPopDataFromBuffer(std::span<const uint8_t> &span) {
+ T value = 0;
+
+ std::memcpy(&value, span.data(), sizeof(T));
+ span = span.subspan(sizeof(T));
+
+ return value;
+}
+
+// Values from the data field mapping table in
+// https://docs.google.com/document/d/12AJUruW7DZ2pIrDzTyPC0qqFoia4QOSVlax6Jd7m4H0/edit?usp=sharing
+void CANTranslator::HandleFrame(const frc971::can_logger::CanFrame *can_frame) {
+ aos::Sender<frc971::imu::DualImuStatic>::StaticBuilder dual_imu_builder =
+ dual_imu_sender_.MakeStaticBuilder();
+
+ std::span can_data(can_frame->data()->data(), kCanFrameSize);
+
+ frc971::imu::SingleImuStatic *murata = dual_imu_builder->add_murata();
+
+ auto *murata_chip_states = murata->add_chip_states();
+ frc971::imu::ChipStateStatic *murata_uno_chip_state =
+ murata_chip_states->emplace_back();
+ frc971::imu::ChipStateStatic *murata_due_chip_state =
+ murata_chip_states->emplace_back();
+
+ frc971::imu::SingleImuStatic *tdk = dual_imu_builder->add_tdk();
+
+ auto tdk_chip_states = tdk->add_chip_states();
+ frc971::imu::ChipStateStatic *tdk_chip_state =
+ tdk_chip_states->emplace_back();
+
+ dual_imu_builder->set_board_timestamp_us(
+ GetAndPopDataFromBuffer<uint32_t>(can_data));
+
+ dual_imu_builder->set_packet_counter(
+ GetAndPopDataFromBuffer<uint16_t>(can_data));
+
+ tdk_chip_state->set_counter(GetAndPopDataFromBuffer<uint16_t>(can_data));
+ murata_uno_chip_state->set_counter(
+ GetAndPopDataFromBuffer<uint16_t>(can_data));
+ murata_due_chip_state->set_counter(
+ GetAndPopDataFromBuffer<uint16_t>(can_data));
+
+ tdk->set_accelerometer_x(GetAndPopDataFromBuffer<float>(can_data));
+ tdk->set_accelerometer_y(GetAndPopDataFromBuffer<float>(can_data));
+ tdk->set_accelerometer_z(GetAndPopDataFromBuffer<float>(can_data));
+
+ tdk->set_gyro_x(GetAndPopDataFromBuffer<float>(can_data));
+ tdk->set_gyro_y(GetAndPopDataFromBuffer<float>(can_data));
+ tdk->set_gyro_z(GetAndPopDataFromBuffer<float>(can_data));
+
+ murata->set_accelerometer_x(GetAndPopDataFromBuffer<float>(can_data));
+ murata->set_accelerometer_y(GetAndPopDataFromBuffer<float>(can_data));
+ murata->set_accelerometer_z(GetAndPopDataFromBuffer<float>(can_data));
+
+ murata->set_gyro_x(GetAndPopDataFromBuffer<float>(can_data));
+ murata->set_gyro_y(GetAndPopDataFromBuffer<float>(can_data));
+ murata->set_gyro_z(GetAndPopDataFromBuffer<float>(can_data));
+
+ tdk_chip_state->set_temperature(GetAndPopDataFromBuffer<uint8_t>(can_data));
+ murata_uno_chip_state->set_temperature(
+ GetAndPopDataFromBuffer<uint8_t>(can_data));
+ murata_due_chip_state->set_temperature(
+ GetAndPopDataFromBuffer<uint8_t>(can_data));
+
+ murata_uno_chip_state->set_max_counter(std::numeric_limits<uint16_t>::max());
+ murata_due_chip_state->set_max_counter(std::numeric_limits<uint16_t>::max());
+ tdk_chip_state->set_max_counter(std::numeric_limits<uint16_t>::max());
+ dual_imu_builder->set_max_packet_counter(
+ std::numeric_limits<uint16_t>::max());
+
+ dual_imu_builder->set_kernel_timestamp(can_frame->monotonic_timestamp_ns());
+
+ dual_imu_builder.CheckOk(dual_imu_builder.Send());
+}
diff --git a/frc971/imu_fdcan/can_translator_lib.h b/frc971/imu_fdcan/can_translator_lib.h
new file mode 100644
index 0000000..47a3638
--- /dev/null
+++ b/frc971/imu_fdcan/can_translator_lib.h
@@ -0,0 +1,30 @@
+#ifndef FRC971_IMU_FDCAN_CAN_TRANSLATOR_LIB_H_
+#define FRC971_IMU_FDCAN_CAN_TRANSLATOR_LIB_H_
+#include "aos/events/event_loop.h"
+#include "frc971/can_logger/can_logging_generated.h"
+#include "frc971/imu_fdcan/can_translator_status_static.h"
+#include "frc971/imu_fdcan/dual_imu_static.h"
+
+namespace frc971::imu_fdcan {
+
+// Translates the CanFrames from the IMU into a DualIMU message based on the
+// spec defined in this doc:
+// https://docs.google.com/document/d/12AJUruW7DZ2pIrDzTyPC0qqFoia4QOSVlax6Jd7m4H0/edit?usp=sharing
+class CANTranslator {
+ public:
+ CANTranslator(aos::EventLoop *event_loop, std::string_view canframe_channel);
+
+ private:
+ void HandleFrame(const can_logger::CanFrame *can_frame);
+
+ aos::Sender<imu::DualImuStatic> dual_imu_sender_;
+ aos::Sender<imu::CanTranslatorStatusStatic> can_translator_status_sender_;
+
+ uint64_t valid_packet_count_ = 0;
+ uint64_t invalid_packet_count_ = 0;
+ uint64_t invalid_can_id_count_ = 0;
+};
+
+} // namespace frc971::imu_fdcan
+
+#endif // FRC971_IMU_FDCAN_CAN_TRANSLATOR_LIB_H_
diff --git a/frc971/imu_fdcan/can_translator_lib_test.cc b/frc971/imu_fdcan/can_translator_lib_test.cc
new file mode 100644
index 0000000..2fb2fe4
--- /dev/null
+++ b/frc971/imu_fdcan/can_translator_lib_test.cc
@@ -0,0 +1,191 @@
+#include "frc971/imu_fdcan/can_translator_lib.h"
+
+#include "glog/logging.h"
+#include "gtest/gtest.h"
+
+#include "aos/events/simulated_event_loop.h"
+#include "frc971/can_logger/can_logging_static.h"
+#include "frc971/imu_fdcan/can_translator_status_generated.h"
+#include "frc971/imu_fdcan/dual_imu_generated.h"
+
+class CANTranslatorTest : public ::testing::Test {
+ public:
+ CANTranslatorTest()
+ : config_(aos::configuration::ReadConfig(
+ "frc971/imu_fdcan/dual_imu_test_config.json")),
+ event_loop_factory_(&config_.message()),
+ can_translator_event_loop_(
+ event_loop_factory_.MakeEventLoop("can_translator")),
+ can_frame_event_loop_(event_loop_factory_.MakeEventLoop("can_frame")),
+ dual_imu_fetcher_(
+ can_translator_event_loop_->MakeFetcher<frc971::imu::DualImu>(
+ "/imu")),
+ can_translator_status_fetcher_(
+ can_translator_event_loop_
+ ->MakeFetcher<frc971::imu::CanTranslatorStatus>("/imu")),
+ can_frame_sender_(
+ can_frame_event_loop_
+ ->MakeSender<frc971::can_logger::CanFrameStatic>("/can")),
+ can_translator_(can_translator_event_loop_.get(), "/can") {}
+
+ protected:
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config_;
+ aos::SimulatedEventLoopFactory event_loop_factory_;
+
+ std::unique_ptr<aos::EventLoop> can_translator_event_loop_;
+ std::unique_ptr<aos::EventLoop> can_frame_event_loop_;
+
+ aos::Fetcher<frc971::imu::DualImu> dual_imu_fetcher_;
+ aos::Fetcher<frc971::imu::CanTranslatorStatus> can_translator_status_fetcher_;
+
+ aos::Sender<frc971::can_logger::CanFrameStatic> can_frame_sender_;
+
+ frc971::imu_fdcan::CANTranslator can_translator_;
+};
+
+TEST_F(CANTranslatorTest, CheckValidFrame) {
+ can_frame_event_loop_->OnRun([this] {
+ aos::Sender<frc971::can_logger::CanFrameStatic>::StaticBuilder
+ can_frame_builder = can_frame_sender_.MakeStaticBuilder();
+
+ can_frame_builder->set_can_id(1);
+ can_frame_builder->set_monotonic_timestamp_ns(100);
+ auto can_data = can_frame_builder->add_data();
+ CHECK(can_data->reserve(sizeof(uint8_t) * 64));
+
+ CHECK(can_data->emplace_back(226));
+ CHECK(can_data->emplace_back(100));
+ CHECK(can_data->emplace_back(108));
+ CHECK(can_data->emplace_back(8));
+ CHECK(can_data->emplace_back(152));
+ CHECK(can_data->emplace_back(40));
+ CHECK(can_data->emplace_back(202));
+ CHECK(can_data->emplace_back(121));
+ CHECK(can_data->emplace_back(202));
+ CHECK(can_data->emplace_back(121));
+ CHECK(can_data->emplace_back(202));
+ CHECK(can_data->emplace_back(121));
+ CHECK(can_data->emplace_back(85));
+ CHECK(can_data->emplace_back(85));
+ CHECK(can_data->emplace_back(81));
+ CHECK(can_data->emplace_back(189));
+ CHECK(can_data->emplace_back(0));
+ CHECK(can_data->emplace_back(0));
+ CHECK(can_data->emplace_back(8));
+ CHECK(can_data->emplace_back(189));
+ CHECK(can_data->emplace_back(85));
+ CHECK(can_data->emplace_back(213));
+ CHECK(can_data->emplace_back(127));
+ CHECK(can_data->emplace_back(191));
+ CHECK(can_data->emplace_back(12));
+ CHECK(can_data->emplace_back(189));
+ CHECK(can_data->emplace_back(34));
+ CHECK(can_data->emplace_back(187));
+ CHECK(can_data->emplace_back(255));
+ CHECK(can_data->emplace_back(219));
+ CHECK(can_data->emplace_back(220));
+ CHECK(can_data->emplace_back(59));
+ CHECK(can_data->emplace_back(147));
+ CHECK(can_data->emplace_back(173));
+ CHECK(can_data->emplace_back(5));
+ CHECK(can_data->emplace_back(61));
+ CHECK(can_data->emplace_back(88));
+ CHECK(can_data->emplace_back(68));
+ CHECK(can_data->emplace_back(205));
+ CHECK(can_data->emplace_back(188));
+ CHECK(can_data->emplace_back(230));
+ CHECK(can_data->emplace_back(92));
+ CHECK(can_data->emplace_back(24));
+ CHECK(can_data->emplace_back(189));
+ CHECK(can_data->emplace_back(235));
+ CHECK(can_data->emplace_back(1));
+ CHECK(can_data->emplace_back(127));
+ CHECK(can_data->emplace_back(191));
+ CHECK(can_data->emplace_back(210));
+ CHECK(can_data->emplace_back(7));
+ CHECK(can_data->emplace_back(34));
+ CHECK(can_data->emplace_back(54));
+ CHECK(can_data->emplace_back(86));
+ CHECK(can_data->emplace_back(103));
+ CHECK(can_data->emplace_back(133));
+ CHECK(can_data->emplace_back(186));
+ CHECK(can_data->emplace_back(100));
+ CHECK(can_data->emplace_back(205));
+ CHECK(can_data->emplace_back(101));
+ CHECK(can_data->emplace_back(185));
+ CHECK(can_data->emplace_back(29));
+ CHECK(can_data->emplace_back(26));
+ CHECK(can_data->emplace_back(26));
+ CHECK(can_data->emplace_back(0));
+
+ can_frame_builder.CheckOk(can_frame_builder.Send());
+ });
+
+ event_loop_factory_.RunFor(std::chrono::milliseconds(200));
+
+ ASSERT_TRUE(can_translator_status_fetcher_.Fetch());
+ ASSERT_TRUE(dual_imu_fetcher_.Fetch());
+
+ ASSERT_FALSE(can_translator_status_fetcher_->invalid_packet_count() > 0);
+ ASSERT_FALSE(can_translator_status_fetcher_->invalid_can_id_count() > 0);
+ EXPECT_EQ(can_translator_status_fetcher_->valid_packet_count(), 1);
+
+ EXPECT_EQ(dual_imu_fetcher_->board_timestamp_us(), 141321442);
+ EXPECT_EQ(dual_imu_fetcher_->packet_counter(), 10392);
+
+ EXPECT_NEAR(dual_imu_fetcher_->murata()->gyro_x(), 2.41444e-06, 0.00001);
+ EXPECT_NEAR(dual_imu_fetcher_->murata()->gyro_y(), -0.00101779, 0.00001);
+ EXPECT_NEAR(dual_imu_fetcher_->murata()->gyro_z(), -0.000219157, 0.00001);
+
+ EXPECT_NEAR(dual_imu_fetcher_->murata()->accelerometer_x(), -0.025057,
+ 0.00001);
+ EXPECT_NEAR(dual_imu_fetcher_->murata()->accelerometer_y(), -0.037198,
+ 0.00001);
+ EXPECT_NEAR(dual_imu_fetcher_->murata()->accelerometer_z(), -0.996123,
+ 0.00001);
+
+ EXPECT_EQ(dual_imu_fetcher_->murata()->chip_states()->Get(0)->counter(),
+ 31178);
+ EXPECT_EQ(dual_imu_fetcher_->murata()->chip_states()->Get(0)->temperature(),
+ 26);
+
+ EXPECT_EQ(dual_imu_fetcher_->murata()->chip_states()->Get(1)->counter(),
+ 31178);
+ EXPECT_EQ(dual_imu_fetcher_->murata()->chip_states()->Get(1)->temperature(),
+ 26);
+
+ EXPECT_NEAR(dual_imu_fetcher_->tdk()->gyro_x(), -0.00248319, 0.00001);
+ EXPECT_NEAR(dual_imu_fetcher_->tdk()->gyro_y(), 0.00674009, 0.00001);
+ EXPECT_NEAR(dual_imu_fetcher_->tdk()->gyro_z(), 0.0326362, 0.00001);
+
+ EXPECT_NEAR(dual_imu_fetcher_->tdk()->accelerometer_x(), -0.0511068, 0.00001);
+ EXPECT_NEAR(dual_imu_fetcher_->tdk()->accelerometer_y(), -0.0332031, 0.00001);
+ EXPECT_NEAR(dual_imu_fetcher_->tdk()->accelerometer_z(), -0.999349, 0.00001);
+
+ EXPECT_EQ(dual_imu_fetcher_->tdk()->chip_states()->Get(0)->counter(), 31178);
+ EXPECT_EQ(dual_imu_fetcher_->tdk()->chip_states()->Get(0)->temperature(), 29);
+}
+
+TEST_F(CANTranslatorTest, CheckInvalidFrame) {
+ can_frame_event_loop_->OnRun([this] {
+ aos::Sender<frc971::can_logger::CanFrameStatic>::StaticBuilder
+ can_frame_builder = can_frame_sender_.MakeStaticBuilder();
+
+ can_frame_builder->set_can_id(2);
+ can_frame_builder->set_monotonic_timestamp_ns(100);
+ auto can_data = can_frame_builder->add_data();
+ CHECK(can_data->reserve(sizeof(uint8_t) * 1));
+
+ CHECK(can_data->emplace_back(0));
+
+ can_frame_builder.CheckOk(can_frame_builder.Send());
+ });
+
+ event_loop_factory_.RunFor(std::chrono::milliseconds(200));
+
+ ASSERT_TRUE(can_translator_status_fetcher_.Fetch());
+ ASSERT_FALSE(dual_imu_fetcher_.Fetch());
+
+ EXPECT_EQ(can_translator_status_fetcher_->invalid_packet_count(), 1);
+ EXPECT_EQ(can_translator_status_fetcher_->invalid_can_id_count(), 1);
+}
diff --git a/frc971/imu_fdcan/can_translator_main.cc b/frc971/imu_fdcan/can_translator_main.cc
new file mode 100644
index 0000000..fdb7107
--- /dev/null
+++ b/frc971/imu_fdcan/can_translator_main.cc
@@ -0,0 +1,22 @@
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "frc971/imu_fdcan/can_translator_lib.h"
+
+DEFINE_string(channel, "/can", "The CAN channel to use");
+
+using frc971::imu_fdcan::CANTranslator;
+
+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());
+
+ CANTranslator translator(&event_loop, FLAGS_channel);
+
+ event_loop.Run();
+
+ return 0;
+}
diff --git a/frc971/imu_fdcan/can_translator_status.fbs b/frc971/imu_fdcan/can_translator_status.fbs
new file mode 100644
index 0000000..232f3ba
--- /dev/null
+++ b/frc971/imu_fdcan/can_translator_status.fbs
@@ -0,0 +1,12 @@
+namespace frc971.imu;
+
+table CanTranslatorStatus {
+ // Number of times we've gotten valid packets at 64 bytes
+ valid_packet_count: uint64 (id: 0);
+ // Number of times we've gotten packets under 64 bytes
+ invalid_packet_count: uint64 (id: 1);
+ // Number of times we've gotten an invalid can id
+ invalid_can_id_count: uint64 (id: 2);
+}
+
+root_type CanTranslatorStatus;
diff --git a/frc971/imu_fdcan/dual_imu.fbs b/frc971/imu_fdcan/dual_imu.fbs
index 7236acf..88f02df 100644
--- a/frc971/imu_fdcan/dual_imu.fbs
+++ b/frc971/imu_fdcan/dual_imu.fbs
@@ -1,5 +1,7 @@
namespace frc971.imu;
+attribute "static_length";
+
table ChipState {
// Counter indicating how many samples have been taken on this IMU.
// Note that because averaging occurs on the microcontroller, this
@@ -28,14 +30,14 @@
accelerometer_z:double (id: 5);
// State for the individual ASIC(s) for this IMU.
- chip_states:[ChipState] (id: 6);
+ chip_states:[ChipState] (id: 6, static_length: 2);
}
table DualImu {
- // Timestamp from the board corresponding to these samples.
+ // Timestamp from the board corresponding to these samples in nanoseconds.
// Future changes may lead us to add per-chip timestamps, but for now
// we treat them as being sampled at the same time.
- board_timestamp_us:uint32 (id: 0);
+ kernel_timestamp:uint64 (id: 0);
// Packet counter that should increment by 1 for every incoming packet.
packet_counter:uint64 (id: 1);
// Value at which the packet counter wraps, such that
@@ -45,6 +47,8 @@
murata:SingleImu (id: 3);
// Readings associated with the TDK IMU.
tdk:SingleImu (id: 4);
+ // Timestamp from the IMU in microseconds
+ board_timestamp_us:uint32 (id: 5);
}
root_type DualImu;
diff --git a/frc971/imu_fdcan/dual_imu_blender_lib.cc b/frc971/imu_fdcan/dual_imu_blender_lib.cc
new file mode 100644
index 0000000..09655e3
--- /dev/null
+++ b/frc971/imu_fdcan/dual_imu_blender_lib.cc
@@ -0,0 +1,128 @@
+#include "frc971/imu_fdcan/dual_imu_blender_lib.h"
+
+#include "gflags/gflags.h"
+
+DEFINE_bool(murata_only, false,
+ "If true then only use the murata value and ignore the tdk.");
+
+// Saturation for the gyro is measured in +- radians/s
+static constexpr double kMurataGyroSaturation = (300.0 * M_PI) / 180;
+
+// Measured in gs
+static constexpr double kMurataAccelSaturation = 6.0;
+
+// Coefficient to multiply the saturation values by to give some room on where
+// we switch to tdk.
+static constexpr double kSaturationCoeff = 0.9;
+
+using frc971::imu_fdcan::DualImuBlender;
+
+DualImuBlender::DualImuBlender(aos::EventLoop *event_loop)
+ : imu_values_batch_sender_(
+ event_loop->MakeSender<frc971::IMUValuesBatchStatic>("/localizer")),
+ dual_imu_blender_status_sender_(
+ event_loop->MakeSender<frc971::imu::DualImuBlenderStatusStatic>(
+ "/imu")) {
+ // TODO(max): Give this a proper priority
+ event_loop->SetRuntimeRealtimePriority(15);
+
+ event_loop->MakeWatcher("/imu", [this](const frc971::imu::DualImu &dual_imu) {
+ HandleDualImu(&dual_imu);
+ });
+}
+
+void DualImuBlender::HandleDualImu(const frc971::imu::DualImu *dual_imu) {
+ aos::Sender<frc971::IMUValuesBatchStatic>::StaticBuilder
+ imu_values_batch_builder_ = imu_values_batch_sender_.MakeStaticBuilder();
+
+ aos::Sender<frc971::imu::DualImuBlenderStatusStatic>::StaticBuilder
+ dual_imu_blender_status_builder =
+ dual_imu_blender_status_sender_.MakeStaticBuilder();
+
+ frc971::IMUValuesStatic *imu_values =
+ CHECK_NOTNULL(imu_values_batch_builder_->add_readings()->emplace_back());
+
+ imu_values->set_pico_timestamp_us(dual_imu->board_timestamp_us());
+ imu_values->set_monotonic_timestamp_ns(dual_imu->kernel_timestamp());
+ imu_values->set_data_counter(dual_imu->packet_counter());
+
+ if (std::abs(dual_imu->tdk()->gyro_x()) >=
+ kSaturationCoeff * kMurataGyroSaturation) {
+ dual_imu_blender_status_builder->set_gyro_x(imu::ImuType::TDK);
+ imu_values->set_gyro_x(dual_imu->tdk()->gyro_x());
+ } else {
+ dual_imu_blender_status_builder->set_gyro_x(imu::ImuType::MURATA);
+ imu_values->set_gyro_x(dual_imu->murata()->gyro_x());
+ }
+
+ if (std::abs(dual_imu->tdk()->gyro_y()) >=
+ kSaturationCoeff * kMurataGyroSaturation) {
+ dual_imu_blender_status_builder->set_gyro_y(imu::ImuType::TDK);
+ imu_values->set_gyro_y(dual_imu->tdk()->gyro_y());
+ } else {
+ dual_imu_blender_status_builder->set_gyro_y(imu::ImuType::MURATA);
+ imu_values->set_gyro_y(dual_imu->murata()->gyro_y());
+ }
+
+ if (std::abs(dual_imu->tdk()->gyro_z()) >=
+ kSaturationCoeff * kMurataGyroSaturation) {
+ dual_imu_blender_status_builder->set_gyro_z(imu::ImuType::TDK);
+ imu_values->set_gyro_z(dual_imu->tdk()->gyro_z());
+ } else {
+ dual_imu_blender_status_builder->set_gyro_z(imu::ImuType::MURATA);
+ imu_values->set_gyro_z(dual_imu->murata()->gyro_z());
+ }
+
+ if (std::abs(dual_imu->tdk()->accelerometer_x()) >=
+ kSaturationCoeff * kMurataAccelSaturation) {
+ dual_imu_blender_status_builder->set_accelerometer_x(imu::ImuType::TDK);
+ imu_values->set_accelerometer_x(dual_imu->tdk()->accelerometer_x());
+ } else {
+ dual_imu_blender_status_builder->set_accelerometer_x(imu::ImuType::MURATA);
+ imu_values->set_accelerometer_x(dual_imu->murata()->accelerometer_x());
+ }
+
+ if (std::abs(dual_imu->tdk()->accelerometer_y()) >=
+ kSaturationCoeff * kMurataAccelSaturation) {
+ dual_imu_blender_status_builder->set_accelerometer_y(imu::ImuType::TDK);
+ imu_values->set_accelerometer_y(dual_imu->tdk()->accelerometer_y());
+ } else {
+ dual_imu_blender_status_builder->set_accelerometer_y(imu::ImuType::MURATA);
+ imu_values->set_accelerometer_y(dual_imu->murata()->accelerometer_y());
+ }
+
+ if (std::abs(dual_imu->tdk()->accelerometer_z()) >=
+ kSaturationCoeff * kMurataAccelSaturation) {
+ dual_imu_blender_status_builder->set_accelerometer_z(imu::ImuType::TDK);
+ imu_values->set_accelerometer_z(dual_imu->tdk()->accelerometer_z());
+ } else {
+ dual_imu_blender_status_builder->set_accelerometer_z(imu::ImuType::MURATA);
+ imu_values->set_accelerometer_z(dual_imu->murata()->accelerometer_z());
+ }
+
+ if (FLAGS_murata_only) {
+ imu_values->set_gyro_x(dual_imu->murata()->gyro_x());
+ imu_values->set_gyro_y(dual_imu->murata()->gyro_y());
+ imu_values->set_gyro_z(dual_imu->murata()->gyro_z());
+
+ imu_values->set_accelerometer_x(dual_imu->murata()->accelerometer_x());
+ imu_values->set_accelerometer_y(dual_imu->murata()->accelerometer_y());
+ imu_values->set_accelerometer_z(dual_imu->murata()->accelerometer_z());
+
+ dual_imu_blender_status_builder->set_gyro_x(imu::ImuType::MURATA);
+ dual_imu_blender_status_builder->set_gyro_y(imu::ImuType::MURATA);
+ dual_imu_blender_status_builder->set_gyro_z(imu::ImuType::MURATA);
+
+ dual_imu_blender_status_builder->set_accelerometer_x(imu::ImuType::MURATA);
+ dual_imu_blender_status_builder->set_accelerometer_y(imu::ImuType::MURATA);
+ dual_imu_blender_status_builder->set_accelerometer_z(imu::ImuType::MURATA);
+ }
+
+ dual_imu_blender_status_builder.CheckOk(
+ dual_imu_blender_status_builder.Send());
+
+ imu_values->set_temperature(
+ dual_imu->murata()->chip_states()->Get(0)->temperature());
+
+ imu_values_batch_builder_.CheckOk(imu_values_batch_builder_.Send());
+}
diff --git a/frc971/imu_fdcan/dual_imu_blender_lib.h b/frc971/imu_fdcan/dual_imu_blender_lib.h
new file mode 100644
index 0000000..04044d2
--- /dev/null
+++ b/frc971/imu_fdcan/dual_imu_blender_lib.h
@@ -0,0 +1,27 @@
+#ifndef FRC971_IMU_FDCAN_DUAL_IMU_BLENDER_H_
+#define FRC971_IMU_FDCAN_DUAL_IMU_BLENDER_H_
+
+#include "aos/events/event_loop.h"
+#include "frc971/imu_fdcan/dual_imu_blender_status_static.h"
+#include "frc971/imu_fdcan/dual_imu_generated.h"
+#include "frc971/wpilib/imu_batch_static.h"
+
+namespace frc971::imu_fdcan {
+
+// Takes in the values from the dual_imu and creates an IMUValuesBatch. Will use
+// the murata until we've hit saturation according to the tdk, then we will
+// switch to using tdk IMU values.
+class DualImuBlender {
+ public:
+ DualImuBlender(aos::EventLoop *event_loop);
+
+ void HandleDualImu(const frc971::imu::DualImu *dual_imu);
+
+ private:
+ aos::Sender<IMUValuesBatchStatic> imu_values_batch_sender_;
+ aos::Sender<imu::DualImuBlenderStatusStatic> dual_imu_blender_status_sender_;
+};
+
+} // namespace frc971::imu_fdcan
+
+#endif // FRC971_IMU_FDCAN_DUAL_IMU_BLENDER_H_
diff --git a/frc971/imu_fdcan/dual_imu_blender_lib_test.cc b/frc971/imu_fdcan/dual_imu_blender_lib_test.cc
new file mode 100644
index 0000000..b3a3015
--- /dev/null
+++ b/frc971/imu_fdcan/dual_imu_blender_lib_test.cc
@@ -0,0 +1,302 @@
+#include "frc971/imu_fdcan/dual_imu_blender_lib.h"
+
+#include "glog/logging.h"
+#include "gtest/gtest.h"
+
+#include "aos/events/simulated_event_loop.h"
+#include "frc971/imu_fdcan/dual_imu_blender_lib.h"
+#include "frc971/imu_fdcan/dual_imu_blender_status_generated.h"
+#include "frc971/imu_fdcan/dual_imu_generated.h"
+#include "frc971/imu_fdcan/dual_imu_static.h"
+
+class DualImuBlenderTest : public ::testing::Test {
+ public:
+ DualImuBlenderTest()
+ : config_(aos::configuration::ReadConfig(
+ "frc971/imu_fdcan/dual_imu_test_config.json")),
+ event_loop_factory_(&config_.message()),
+ dual_imu_blender_event_loop_(
+ event_loop_factory_.MakeEventLoop("dual_imu_blender")),
+ dual_imu_event_loop_(event_loop_factory_.MakeEventLoop("dual_imu")),
+ imu_values_batch_fetcher_(
+ dual_imu_event_loop_->MakeFetcher<frc971::IMUValuesBatch>(
+ "/localizer")),
+ dual_imu_blender_status_fetcher_(
+ dual_imu_blender_event_loop_
+ ->MakeFetcher<frc971::imu::DualImuBlenderStatus>("/imu")),
+ dual_imu_sender_(
+ dual_imu_event_loop_->MakeSender<frc971::imu::DualImuStatic>(
+ "/imu")),
+ dual_imu_blender_(dual_imu_blender_event_loop_.get()) {}
+
+ void CheckImuType(frc971::imu::ImuType type) {
+ EXPECT_EQ(dual_imu_blender_status_fetcher_->gyro_x(), type);
+ EXPECT_EQ(dual_imu_blender_status_fetcher_->gyro_y(), type);
+ EXPECT_EQ(dual_imu_blender_status_fetcher_->gyro_z(), type);
+ EXPECT_EQ(dual_imu_blender_status_fetcher_->accelerometer_x(), type);
+ EXPECT_EQ(dual_imu_blender_status_fetcher_->accelerometer_y(), type);
+ EXPECT_EQ(dual_imu_blender_status_fetcher_->accelerometer_z(), type);
+ }
+
+ protected:
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config_;
+ aos::SimulatedEventLoopFactory event_loop_factory_;
+
+ std::unique_ptr<aos::EventLoop> dual_imu_blender_event_loop_;
+ std::unique_ptr<aos::EventLoop> dual_imu_event_loop_;
+
+ aos::Fetcher<frc971::IMUValuesBatch> imu_values_batch_fetcher_;
+ aos::Fetcher<frc971::imu::DualImuBlenderStatus>
+ dual_imu_blender_status_fetcher_;
+
+ aos::Sender<frc971::imu::DualImuStatic> dual_imu_sender_;
+
+ frc971::imu_fdcan::DualImuBlender dual_imu_blender_;
+};
+
+// Sanity check that some sane values in are the same values out
+TEST_F(DualImuBlenderTest, SanityCheck) {
+ dual_imu_blender_event_loop_->OnRun([this] {
+ aos::Sender<frc971::imu::DualImuStatic>::StaticBuilder dual_imu_builder =
+ dual_imu_sender_.MakeStaticBuilder();
+
+ frc971::imu::SingleImuStatic *murata = dual_imu_builder->add_murata();
+
+ auto *murata_chip_states = murata->add_chip_states();
+ frc971::imu::ChipStateStatic *murata_uno_chip_state =
+ murata_chip_states->emplace_back();
+ frc971::imu::ChipStateStatic *murata_due_chip_state =
+ murata_chip_states->emplace_back();
+
+ frc971::imu::SingleImuStatic *tdk = dual_imu_builder->add_tdk();
+
+ dual_imu_builder->set_board_timestamp_us(0);
+ dual_imu_builder->set_kernel_timestamp(0);
+
+ tdk->set_gyro_x(0.3);
+ tdk->set_gyro_y(0.2);
+ tdk->set_gyro_z(0.2);
+
+ murata->set_gyro_x(0.351);
+ murata->set_gyro_y(0.284);
+ murata->set_gyro_z(0.293);
+
+ tdk->set_accelerometer_x(1.5);
+ tdk->set_accelerometer_y(1.5);
+ tdk->set_accelerometer_z(1.5);
+
+ murata->set_accelerometer_x(1.58);
+ murata->set_accelerometer_y(1.51);
+ murata->set_accelerometer_z(1.52);
+
+ murata_uno_chip_state->set_temperature(20);
+ murata_due_chip_state->set_temperature(10);
+
+ dual_imu_builder.CheckOk(dual_imu_builder.Send());
+ });
+
+ event_loop_factory_.RunFor(std::chrono::milliseconds(200));
+
+ ASSERT_TRUE(imu_values_batch_fetcher_.Fetch());
+ ASSERT_TRUE(dual_imu_blender_status_fetcher_.Fetch());
+
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->gyro_x(), 0.351,
+ 0.0001);
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->gyro_y(), 0.284,
+ 0.0001);
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->gyro_z(), 0.293,
+ 0.0001);
+
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->accelerometer_x(),
+ 1.58, 0.0001);
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->accelerometer_y(),
+ 1.51, 0.0001);
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->accelerometer_z(),
+ 1.52, 0.0001);
+
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->temperature(), 20,
+ 0.0001);
+
+ CheckImuType(frc971::imu::ImuType::MURATA);
+}
+
+TEST_F(DualImuBlenderTest, Saturation) {
+ dual_imu_blender_event_loop_->OnRun([this] {
+ aos::Sender<frc971::imu::DualImuStatic>::StaticBuilder dual_imu_builder =
+ dual_imu_sender_.MakeStaticBuilder();
+
+ frc971::imu::SingleImuStatic *murata = dual_imu_builder->add_murata();
+
+ auto *murata_chip_states = murata->add_chip_states();
+ frc971::imu::ChipStateStatic *murata_uno_chip_state =
+ murata_chip_states->emplace_back();
+
+ frc971::imu::SingleImuStatic *tdk = dual_imu_builder->add_tdk();
+
+ dual_imu_builder->set_board_timestamp_us(0);
+ dual_imu_builder->set_kernel_timestamp(0);
+
+ tdk->set_gyro_x(0.7);
+ tdk->set_gyro_y(0.7);
+ tdk->set_gyro_z(0.7);
+
+ murata->set_gyro_x(0.71);
+ murata->set_gyro_y(0.79);
+ murata->set_gyro_z(0.78);
+
+ tdk->set_accelerometer_x(1.0);
+ tdk->set_accelerometer_y(1.0);
+ tdk->set_accelerometer_z(1.0);
+
+ murata->set_accelerometer_x(1.3);
+ murata->set_accelerometer_y(1.1);
+ murata->set_accelerometer_z(1.1);
+
+ murata_uno_chip_state->set_temperature(20);
+
+ dual_imu_builder.CheckOk(dual_imu_builder.Send());
+ });
+
+ event_loop_factory_.RunFor(std::chrono::milliseconds(200));
+
+ ASSERT_TRUE(imu_values_batch_fetcher_.Fetch());
+ ASSERT_TRUE(dual_imu_blender_status_fetcher_.Fetch());
+
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->gyro_x(), 0.71,
+ 0.0001);
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->gyro_y(), 0.79,
+ 0.0001);
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->gyro_z(), 0.78,
+ 0.0001);
+
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->accelerometer_x(),
+ 1.3, 0.0001);
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->accelerometer_y(),
+ 1.1, 0.0001);
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->accelerometer_z(),
+ 1.1, 0.0001);
+
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->temperature(), 20,
+ 0.0001);
+
+ CheckImuType(frc971::imu::ImuType::MURATA);
+
+ // Make sure we switch to TDK on saturation
+ dual_imu_blender_event_loop_->OnRun([this] {
+ aos::Sender<frc971::imu::DualImuStatic>::StaticBuilder dual_imu_builder =
+ dual_imu_sender_.MakeStaticBuilder();
+
+ frc971::imu::SingleImuStatic *murata = dual_imu_builder->add_murata();
+
+ auto *murata_chip_states = murata->add_chip_states();
+ frc971::imu::ChipStateStatic *murata_uno_chip_state =
+ murata_chip_states->emplace_back();
+
+ frc971::imu::SingleImuStatic *tdk = dual_imu_builder->add_tdk();
+
+ dual_imu_builder->set_board_timestamp_us(1);
+ dual_imu_builder->set_kernel_timestamp(1);
+
+ tdk->set_gyro_x(6.0);
+ tdk->set_gyro_y(6.0);
+ tdk->set_gyro_z(6.0);
+
+ murata->set_gyro_x(5.2);
+ murata->set_gyro_y(5.2);
+ murata->set_gyro_z(5.2);
+
+ tdk->set_accelerometer_x(6.2);
+ tdk->set_accelerometer_y(6.3);
+ tdk->set_accelerometer_z(6.5);
+
+ murata->set_accelerometer_x(5.5);
+ murata->set_accelerometer_y(5.5);
+ murata->set_accelerometer_z(5.5);
+
+ murata_uno_chip_state->set_temperature(20);
+
+ dual_imu_builder.CheckOk(dual_imu_builder.Send());
+ });
+
+ event_loop_factory_.RunFor(std::chrono::milliseconds(200));
+
+ ASSERT_TRUE(imu_values_batch_fetcher_.Fetch());
+ ASSERT_TRUE(dual_imu_blender_status_fetcher_.Fetch());
+
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->gyro_x(), 6.0,
+ 0.0001);
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->gyro_y(), 6.0,
+ 0.0001);
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->gyro_z(), 6.0,
+ 0.0001);
+
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->accelerometer_x(),
+ 6.2, 0.0001);
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->accelerometer_y(),
+ 6.3, 0.0001);
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->accelerometer_z(),
+ 6.5, 0.0001);
+
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->temperature(), 20,
+ 0.0001);
+
+ CheckImuType(frc971::imu::ImuType::TDK);
+
+ // Check negative values as well
+ dual_imu_blender_event_loop_->OnRun([this] {
+ aos::Sender<frc971::imu::DualImuStatic>::StaticBuilder dual_imu_builder =
+ dual_imu_sender_.MakeStaticBuilder();
+
+ frc971::imu::SingleImuStatic *murata = dual_imu_builder->add_murata();
+
+ auto *murata_chip_states = murata->add_chip_states();
+ frc971::imu::ChipStateStatic *murata_uno_chip_state =
+ murata_chip_states->emplace_back();
+
+ frc971::imu::SingleImuStatic *tdk = dual_imu_builder->add_tdk();
+
+ dual_imu_builder->set_board_timestamp_us(1);
+ dual_imu_builder->set_kernel_timestamp(1);
+
+ tdk->set_gyro_x(-6.0);
+ tdk->set_gyro_y(-6.0);
+ tdk->set_gyro_z(-6.0);
+
+ murata->set_gyro_x(-5.2);
+ murata->set_gyro_y(-5.2);
+ murata->set_gyro_z(-5.2);
+
+ tdk->set_accelerometer_x(-6.2);
+ tdk->set_accelerometer_y(-6.3);
+ tdk->set_accelerometer_z(-6.5);
+
+ murata->set_accelerometer_x(-5.5);
+ murata->set_accelerometer_y(-5.5);
+ murata->set_accelerometer_z(-5.5);
+
+ murata_uno_chip_state->set_temperature(20);
+
+ dual_imu_builder.CheckOk(dual_imu_builder.Send());
+ });
+
+ event_loop_factory_.RunFor(std::chrono::milliseconds(200));
+
+ ASSERT_TRUE(imu_values_batch_fetcher_.Fetch());
+ ASSERT_TRUE(dual_imu_blender_status_fetcher_.Fetch());
+
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->gyro_x(), -6.0,
+ 0.0001);
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->gyro_y(), -6.0,
+ 0.0001);
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->gyro_z(), -6.0,
+ 0.0001);
+
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->accelerometer_x(),
+ -6.2, 0.0001);
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->accelerometer_y(),
+ -6.3, 0.0001);
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->accelerometer_z(),
+ -6.5, 0.0001);
+
+ CheckImuType(frc971::imu::ImuType::TDK);
+}
diff --git a/frc971/imu_fdcan/dual_imu_blender_main.cc b/frc971/imu_fdcan/dual_imu_blender_main.cc
new file mode 100644
index 0000000..1d5ee22
--- /dev/null
+++ b/frc971/imu_fdcan/dual_imu_blender_main.cc
@@ -0,0 +1,20 @@
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "frc971/imu_fdcan/dual_imu_blender_lib.h"
+
+using frc971::imu_fdcan::DualImuBlender;
+
+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());
+
+ DualImuBlender blender(&event_loop);
+
+ event_loop.Run();
+
+ return 0;
+}
diff --git a/frc971/imu_fdcan/dual_imu_blender_status.fbs b/frc971/imu_fdcan/dual_imu_blender_status.fbs
new file mode 100644
index 0000000..e3c9893
--- /dev/null
+++ b/frc971/imu_fdcan/dual_imu_blender_status.fbs
@@ -0,0 +1,19 @@
+namespace frc971.imu;
+
+enum ImuType : ubyte {
+ MURATA = 0,
+ TDK = 1,
+}
+
+table DualImuBlenderStatus {
+ // These values explain if we're using the tdk or the murata for our accelerometers and gyro.
+ gyro_x: ImuType (id: 0);
+ gyro_y: ImuType (id: 1);
+ gyro_z: ImuType (id: 2);
+
+ accelerometer_x: ImuType (id: 3);
+ accelerometer_y: ImuType (id: 4);
+ accelerometer_z: ImuType (id: 5);
+}
+
+root_type DualImuBlenderStatus;
diff --git a/frc971/imu_fdcan/dual_imu_test_config_source.json b/frc971/imu_fdcan/dual_imu_test_config_source.json
new file mode 100644
index 0000000..eda06da
--- /dev/null
+++ b/frc971/imu_fdcan/dual_imu_test_config_source.json
@@ -0,0 +1,38 @@
+{
+ "channels": [
+ {
+ "name": "/aos",
+ "type": "aos.timing.Report"
+ },
+ {
+ "name": "/aos",
+ "type": "aos.logging.LogMessageFbs",
+ "frequency": 400
+ },
+ {
+ "name": "/imu",
+ "type": "frc971.imu.DualImu",
+ "frequency": 200
+ },
+ {
+ "name": "/can",
+ "type": "frc971.can_logger.CanFrame",
+ "frequency": 200
+ },
+ {
+ "name": "/localizer",
+ "type": "frc971.IMUValuesBatch",
+ "frequency": 1100
+ },
+ {
+ "name": "/imu",
+ "type": "frc971.imu.CanTranslatorStatus",
+ "frequency": 100
+ },
+ {
+ "name": "/imu",
+ "type": "frc971.imu.DualImuBlenderStatus",
+ "frequency": 100
+ },
+ ]
+}
diff --git a/frc971/vision/target_map_utils.cc b/frc971/vision/target_map_utils.cc
index 232c2b6..3800628 100644
--- a/frc971/vision/target_map_utils.cc
+++ b/frc971/vision/target_map_utils.cc
@@ -8,7 +8,8 @@
return (Eigen::Translation3d(
Eigen::Vector3d(position->x(), position->y(), position->z())) *
Eigen::Quaterniond(quaternion->w(), quaternion->x(), quaternion->y(),
- quaternion->z()))
+ quaternion->z())
+ .normalized())
.matrix();
}
} // namespace frc971::vision
diff --git a/frc971/vision/target_mapper.cc b/frc971/vision/target_mapper.cc
index 8077f16..c14b032 100644
--- a/frc971/vision/target_mapper.cc
+++ b/frc971/vision/target_mapper.cc
@@ -104,7 +104,8 @@
Eigen::Quaterniond(target_pose_fbs.orientation()->w(),
target_pose_fbs.orientation()->x(),
target_pose_fbs.orientation()->y(),
- target_pose_fbs.orientation()->z())}};
+ target_pose_fbs.orientation()->z())
+ .normalized()}};
}
ceres::examples::VectorOfConstraints DataAdapter::MatchTargetDetections(
diff --git a/frc971/wpilib/encoder_and_potentiometer.h b/frc971/wpilib/encoder_and_potentiometer.h
index a5eac24..84bfb64 100644
--- a/frc971/wpilib/encoder_and_potentiometer.h
+++ b/frc971/wpilib/encoder_and_potentiometer.h
@@ -9,6 +9,7 @@
#include "aos/time/time.h"
#include "frc971/wpilib/ahal/AnalogInput.h"
#include "frc971/wpilib/ahal/Counter.h"
+#include "frc971/wpilib/ahal/DigitalInput.h"
#include "frc971/wpilib/ahal/DigitalSource.h"
#include "frc971/wpilib/ahal/Encoder.h"
#include "frc971/wpilib/dma.h"
@@ -168,6 +169,38 @@
::std::unique_ptr<::frc::DigitalInput> input_;
};
+class DMAAbsoluteEncoderAndPotentiometer {
+ public:
+ void set_absolute_pwm(::std::unique_ptr<frc::DigitalInput> input) {
+ duty_cycle_input_ = ::std::move(input);
+ duty_cycle_reader_.set_input(duty_cycle_input_.get());
+ }
+
+ void set_encoder(::std::unique_ptr<frc::Encoder> encoder) {
+ encoder_ = ::std::move(encoder);
+ }
+
+ void set_potentiometer(::std::unique_ptr<frc::AnalogInput> potentiometer) {
+ potentiometer_ = ::std::move(potentiometer);
+ }
+
+ double ReadAbsoluteEncoder() const {
+ return duty_cycle_reader_.last_width() / duty_cycle_reader_.last_period();
+ }
+ int32_t ReadRelativeEncoder() const { return encoder_->GetRaw(); }
+ double ReadPotentiometerVoltage() const {
+ return potentiometer_->GetVoltage();
+ }
+
+ DMAPulseWidthReader &reader() { return duty_cycle_reader_; }
+
+ private:
+ DMAPulseWidthReader duty_cycle_reader_;
+ ::std::unique_ptr<::frc::DigitalInput> duty_cycle_input_;
+ ::std::unique_ptr<frc::Encoder> encoder_;
+ ::std::unique_ptr<frc::AnalogInput> potentiometer_;
+};
+
// Class to hold a CTRE encoder with absolute angle pwm and potentiometer pair.
class AbsoluteEncoderAndPotentiometer {
public:
diff --git a/frc971/wpilib/imu_batch.fbs b/frc971/wpilib/imu_batch.fbs
index 1483314..bead5df 100644
--- a/frc971/wpilib/imu_batch.fbs
+++ b/frc971/wpilib/imu_batch.fbs
@@ -2,8 +2,10 @@
namespace frc971;
+attribute "static_length";
+
table IMUValuesBatch {
- readings:[IMUValues] (id: 0);
+ readings:[IMUValues] (id: 0, static_length: 1);
}
root_type IMUValuesBatch;
diff --git a/frc971/wpilib/sensor_reader.h b/frc971/wpilib/sensor_reader.h
index 455ed0e..da650a5 100644
--- a/frc971/wpilib/sensor_reader.h
+++ b/frc971/wpilib/sensor_reader.h
@@ -102,6 +102,27 @@
encoder_ratio * (2.0 * M_PI));
}
+ void CopyPosition(
+ const ::frc971::wpilib::DMAAbsoluteEncoderAndPotentiometer &encoder,
+ ::frc971::PotAndAbsolutePositionStatic *position,
+ double encoder_counts_per_revolution, double encoder_ratio,
+ ::std::function<double(double)> potentiometer_translate, bool reverse,
+ double pot_offset) {
+ const double multiplier = reverse ? -1.0 : 1.0;
+ position->set_pot(multiplier * potentiometer_translate(
+ encoder.ReadPotentiometerVoltage()) +
+ pot_offset);
+ position->set_encoder(multiplier *
+ encoder_translate(encoder.ReadRelativeEncoder(),
+ encoder_counts_per_revolution,
+ encoder_ratio));
+
+ position->set_absolute_encoder((reverse
+ ? (1.0 - encoder.ReadAbsoluteEncoder())
+ : encoder.ReadAbsoluteEncoder()) *
+ encoder_ratio * (2.0 * M_PI));
+ }
+
// Copies an AbsoluteEncoderAndPotentiometer to an AbsoluteAndAbsolutePosition
// with the correct unit and direction changes.
void CopyPosition(const ::frc971::wpilib::AbsoluteAndAbsoluteEncoder &encoder,
diff --git a/scouting/db/db.go b/scouting/db/db.go
index 759f7e1..5d7a56c 100644
--- a/scouting/db/db.go
+++ b/scouting/db/db.go
@@ -86,7 +86,7 @@
NotesDropped int32
Penalties int32
AvgCycle int64
- Park, OnStage, Harmony, TrapNote bool
+ Park, OnStage, Harmony, TrapNote, Spotlight bool
// The username of the person who collected these statistics.
// "unknown" if submitted without logging in.
diff --git a/scouting/db/db_test.go b/scouting/db/db_test.go
index 4acbe12..369ea58 100644
--- a/scouting/db/db_test.go
+++ b/scouting/db/db_test.go
@@ -148,7 +148,7 @@
MatchNumber: 3, SetNumber: 1, CompLevel: "quals", StartingQuadrant: 4,
SpeakerAuto: 1, AmpAuto: 0, NotesDroppedAuto: 2, MobilityAuto: true,
Speaker: 0, Amp: 5, SpeakerAmplified: 1, AmpAmplified: 0,
- NotesDropped: 0, Penalties: 2, TrapNote: true, AvgCycle: 0,
+ NotesDropped: 0, Penalties: 2, TrapNote: true, Spotlight: false, AvgCycle: 0,
Park: true, OnStage: false, Harmony: false, CollectedBy: "emma",
},
Stats2024{
@@ -156,7 +156,7 @@
MatchNumber: 3, SetNumber: 1, CompLevel: "quals", StartingQuadrant: 2,
SpeakerAuto: 2, AmpAuto: 0, NotesDroppedAuto: 2, MobilityAuto: true,
Speaker: 0, Amp: 5, SpeakerAmplified: 1, AmpAmplified: 0,
- NotesDropped: 0, Penalties: 2, TrapNote: true, AvgCycle: 0,
+ NotesDropped: 0, Penalties: 2, TrapNote: true, Spotlight: false, AvgCycle: 0,
Park: true, OnStage: false, Harmony: false, CollectedBy: "harry",
},
Stats2024{
@@ -164,7 +164,7 @@
MatchNumber: 3, SetNumber: 1, CompLevel: "quals", StartingQuadrant: 3,
SpeakerAuto: 0, AmpAuto: 0, NotesDroppedAuto: 2, MobilityAuto: true,
Speaker: 2, Amp: 1, SpeakerAmplified: 3, AmpAmplified: 0,
- NotesDropped: 0, Penalties: 0, TrapNote: false, AvgCycle: 0,
+ NotesDropped: 0, Penalties: 0, TrapNote: false, Spotlight: false, AvgCycle: 0,
Park: false, OnStage: true, Harmony: false, CollectedBy: "henry",
},
Stats2024{
@@ -172,7 +172,7 @@
MatchNumber: 3, SetNumber: 1, CompLevel: "quals", StartingQuadrant: 1,
SpeakerAuto: 1, AmpAuto: 0, NotesDroppedAuto: 0, MobilityAuto: false,
Speaker: 0, Amp: 1, SpeakerAmplified: 2, AmpAmplified: 3,
- NotesDropped: 2, Penalties: 0, TrapNote: true, AvgCycle: 0,
+ NotesDropped: 2, Penalties: 0, TrapNote: true, Spotlight: false, AvgCycle: 0,
Park: true, OnStage: false, Harmony: false, CollectedBy: "jordan",
},
Stats2024{
@@ -180,7 +180,7 @@
MatchNumber: 3, SetNumber: 1, CompLevel: "quals", StartingQuadrant: 2,
SpeakerAuto: 0, AmpAuto: 0, NotesDroppedAuto: 0, MobilityAuto: false,
Speaker: 5, Amp: 0, SpeakerAmplified: 2, AmpAmplified: 1,
- NotesDropped: 1, Penalties: 1, TrapNote: true, AvgCycle: 0,
+ NotesDropped: 1, Penalties: 1, TrapNote: true, Spotlight: true, AvgCycle: 0,
Park: true, OnStage: false, Harmony: false, CollectedBy: "taylor",
},
Stats2024{
@@ -188,7 +188,7 @@
MatchNumber: 3, SetNumber: 1, CompLevel: "quals", StartingQuadrant: 3,
SpeakerAuto: 1, AmpAuto: 3, NotesDroppedAuto: 0, MobilityAuto: true,
Speaker: 0, Amp: 3, SpeakerAmplified: 2, AmpAmplified: 2,
- NotesDropped: 0, Penalties: 3, TrapNote: true, AvgCycle: 0,
+ NotesDropped: 0, Penalties: 3, TrapNote: true, Spotlight: false, AvgCycle: 0,
Park: false, OnStage: false, Harmony: true, CollectedBy: "katie",
},
}
@@ -235,7 +235,7 @@
MatchNumber: 3, SetNumber: 1, CompLevel: "quals", StartingQuadrant: 4,
SpeakerAuto: 1, AmpAuto: 0, NotesDroppedAuto: 2, MobilityAuto: true,
Speaker: 0, Amp: 5, SpeakerAmplified: 1, AmpAmplified: 0,
- NotesDropped: 0, Penalties: 2, TrapNote: true, AvgCycle: 0,
+ NotesDropped: 0, Penalties: 2, TrapNote: true, Spotlight: true, AvgCycle: 0,
Park: true, OnStage: false, Harmony: false, CollectedBy: "emma",
}
@@ -264,7 +264,7 @@
MatchNumber: 7, SetNumber: 1, CompLevel: "qm", StartingQuadrant: 1,
SpeakerAuto: 1, AmpAuto: 0, NotesDroppedAuto: 2, MobilityAuto: true,
Speaker: 0, Amp: 5, SpeakerAmplified: 1, AmpAmplified: 0,
- NotesDropped: 0, Penalties: 2, TrapNote: true, AvgCycle: 0,
+ NotesDropped: 0, Penalties: 2, TrapNote: true, Spotlight: true, AvgCycle: 0,
Park: false, OnStage: true, Harmony: false, CollectedBy: "emma",
},
Stats2024{
@@ -272,7 +272,7 @@
MatchNumber: 2, SetNumber: 2, CompLevel: "qm", StartingQuadrant: 4,
SpeakerAuto: 0, AmpAuto: 0, NotesDroppedAuto: 0, MobilityAuto: false,
Speaker: 1, Amp: 2, SpeakerAmplified: 0, AmpAmplified: 2,
- NotesDropped: 0, Penalties: 2, TrapNote: true, AvgCycle: 0,
+ NotesDropped: 0, Penalties: 2, TrapNote: true, Spotlight: true, AvgCycle: 0,
Park: true, OnStage: false, Harmony: false, CollectedBy: "emma",
},
Stats2024{
@@ -280,7 +280,7 @@
MatchNumber: 4, SetNumber: 1, CompLevel: "qm", StartingQuadrant: 2,
SpeakerAuto: 1, AmpAuto: 1, NotesDroppedAuto: 1, MobilityAuto: true,
Speaker: 0, Amp: 1, SpeakerAmplified: 1, AmpAmplified: 5,
- NotesDropped: 1, Penalties: 0, TrapNote: false, AvgCycle: 0,
+ NotesDropped: 1, Penalties: 0, TrapNote: false, Spotlight: true, AvgCycle: 0,
Park: false, OnStage: false, Harmony: true, CollectedBy: "emma",
},
}
@@ -699,7 +699,7 @@
MatchNumber: 5, SetNumber: 1, CompLevel: "quals", StartingQuadrant: 1,
SpeakerAuto: 1, AmpAuto: 0, NotesDroppedAuto: 2, MobilityAuto: true,
Speaker: 1, Amp: 3, SpeakerAmplified: 1, AmpAmplified: 3,
- NotesDropped: 0, Penalties: 0, TrapNote: false, AvgCycle: 0,
+ NotesDropped: 0, Penalties: 0, TrapNote: false, Spotlight: false, AvgCycle: 0,
Park: false, OnStage: true, Harmony: false, CollectedBy: "bailey",
},
Stats2024{
@@ -707,7 +707,7 @@
MatchNumber: 5, SetNumber: 1, CompLevel: "quals", StartingQuadrant: 4,
SpeakerAuto: 1, AmpAuto: 0, NotesDroppedAuto: 0, MobilityAuto: false,
Speaker: 1, Amp: 2, SpeakerAmplified: 0, AmpAmplified: 1,
- NotesDropped: 0, Penalties: 2, TrapNote: true, AvgCycle: 0,
+ NotesDropped: 0, Penalties: 2, TrapNote: true, Spotlight: true, AvgCycle: 0,
Park: true, OnStage: false, Harmony: false, CollectedBy: "kate",
},
Stats2024{
@@ -715,7 +715,7 @@
MatchNumber: 5, SetNumber: 1, CompLevel: "quals", StartingQuadrant: 2,
SpeakerAuto: 1, AmpAuto: 1, NotesDroppedAuto: 1, MobilityAuto: true,
Speaker: 0, Amp: 0, SpeakerAmplified: 2, AmpAmplified: 1,
- NotesDropped: 1, Penalties: 0, TrapNote: false, AvgCycle: 0,
+ NotesDropped: 1, Penalties: 0, TrapNote: false, Spotlight: false, AvgCycle: 0,
Park: true, OnStage: false, Harmony: false, CollectedBy: "tyler",
},
Stats2024{
@@ -723,7 +723,7 @@
MatchNumber: 5, SetNumber: 1, CompLevel: "quals", StartingQuadrant: 1,
SpeakerAuto: 1, AmpAuto: 1, NotesDroppedAuto: 0, MobilityAuto: false,
Speaker: 1, Amp: 2, SpeakerAmplified: 2, AmpAmplified: 1,
- NotesDropped: 1, Penalties: 0, TrapNote: false, AvgCycle: 0,
+ NotesDropped: 1, Penalties: 0, TrapNote: false, Spotlight: false, AvgCycle: 0,
Park: false, OnStage: false, Harmony: true, CollectedBy: "max",
},
}
@@ -734,7 +734,7 @@
MatchNumber: 5, SetNumber: 1, CompLevel: "quals", StartingQuadrant: 1,
SpeakerAuto: 1, AmpAuto: 0, NotesDroppedAuto: 2, MobilityAuto: true,
Speaker: 1, Amp: 3, SpeakerAmplified: 1, AmpAmplified: 3,
- NotesDropped: 0, Penalties: 0, TrapNote: false, AvgCycle: 0,
+ NotesDropped: 0, Penalties: 0, TrapNote: false, Spotlight: false, AvgCycle: 0,
Park: false, OnStage: true, Harmony: false, CollectedBy: "bailey",
},
}
@@ -1153,7 +1153,7 @@
MatchNumber: 3, SetNumber: 1, CompLevel: "quals", StartingQuadrant: 4,
SpeakerAuto: 1, AmpAuto: 0, NotesDroppedAuto: 2, MobilityAuto: true,
Speaker: 0, Amp: 5, SpeakerAmplified: 1, AmpAmplified: 0,
- NotesDropped: 0, Penalties: 2, TrapNote: true, AvgCycle: 0,
+ NotesDropped: 0, Penalties: 2, TrapNote: true, Spotlight: false, AvgCycle: 0,
Park: true, OnStage: false, Harmony: false, CollectedBy: "emma",
},
Stats2024{
@@ -1161,7 +1161,7 @@
MatchNumber: 3, SetNumber: 1, CompLevel: "quals", StartingQuadrant: 2,
SpeakerAuto: 2, AmpAuto: 0, NotesDroppedAuto: 2, MobilityAuto: true,
Speaker: 0, Amp: 5, SpeakerAmplified: 1, AmpAmplified: 0,
- NotesDropped: 0, Penalties: 2, TrapNote: true, AvgCycle: 0,
+ NotesDropped: 0, Penalties: 2, TrapNote: true, Spotlight: false, AvgCycle: 0,
Park: true, OnStage: false, Harmony: false, CollectedBy: "harry",
},
Stats2024{
@@ -1169,7 +1169,7 @@
MatchNumber: 3, SetNumber: 1, CompLevel: "quals", StartingQuadrant: 3,
SpeakerAuto: 0, AmpAuto: 0, NotesDroppedAuto: 2, MobilityAuto: true,
Speaker: 2, Amp: 1, SpeakerAmplified: 3, AmpAmplified: 0,
- NotesDropped: 0, Penalties: 0, TrapNote: false, AvgCycle: 0,
+ NotesDropped: 0, Penalties: 0, TrapNote: false, Spotlight: false, AvgCycle: 0,
Park: false, OnStage: true, Harmony: false, CollectedBy: "henry",
},
Stats2024{
@@ -1177,7 +1177,7 @@
MatchNumber: 3, SetNumber: 1, CompLevel: "quals", StartingQuadrant: 1,
SpeakerAuto: 1, AmpAuto: 0, NotesDroppedAuto: 0, MobilityAuto: false,
Speaker: 0, Amp: 1, SpeakerAmplified: 2, AmpAmplified: 3,
- NotesDropped: 2, Penalties: 0, TrapNote: true, AvgCycle: 0,
+ NotesDropped: 2, Penalties: 0, TrapNote: true, Spotlight: true, AvgCycle: 0,
Park: true, OnStage: false, Harmony: false, CollectedBy: "jordan",
},
}
diff --git a/scouting/scouting_test.cy.js b/scouting/scouting_test.cy.js
index 691d8fc..31ca935 100644
--- a/scouting/scouting_test.cy.js
+++ b/scouting/scouting_test.cy.js
@@ -111,7 +111,7 @@
.last()
.should(
'have.text',
- ' Ended Match; park: false, onStage: false, harmony: true, trapNote: false '
+ ' Ended Match; stageType: kHARMONY, trapNote: false, spotlight: false '
);
clickButton('Submit');
diff --git a/scouting/webserver/requests/messages/request_2024_data_scouting_response.fbs b/scouting/webserver/requests/messages/request_2024_data_scouting_response.fbs
index d653f04..c36174d 100644
--- a/scouting/webserver/requests/messages/request_2024_data_scouting_response.fbs
+++ b/scouting/webserver/requests/messages/request_2024_data_scouting_response.fbs
@@ -25,6 +25,7 @@
park: bool (id:15);
on_stage: bool (id:16);
harmony: bool (id:17);
+ spotlight: bool (id:22);
pre_scouting:bool (id:20);
collected_by:string (id:21);
@@ -34,4 +35,4 @@
stats_list:[Stats2024] (id:0);
}
-root_type Request2024DataScoutingResponse;
+root_type Request2024DataScoutingResponse;
\ No newline at end of file
diff --git a/scouting/webserver/requests/messages/submit_2024_actions.fbs b/scouting/webserver/requests/messages/submit_2024_actions.fbs
index 61e36bc..e85563f 100644
--- a/scouting/webserver/requests/messages/submit_2024_actions.fbs
+++ b/scouting/webserver/requests/messages/submit_2024_actions.fbs
@@ -42,6 +42,7 @@
table EndMatchAction {
stage_type:StageType (id:0);
trap_note:bool (id:1);
+ spotlight:bool (id:2);
}
union ActionType {
diff --git a/scouting/webserver/requests/requests.go b/scouting/webserver/requests/requests.go
index 31ad4e3..a9919af 100644
--- a/scouting/webserver/requests/requests.go
+++ b/scouting/webserver/requests/requests.go
@@ -452,7 +452,7 @@
PreScouting: submit2024Actions.PreScouting(), TeamNumber: string(submit2024Actions.TeamNumber()), MatchNumber: submit2024Actions.MatchNumber(), SetNumber: submit2024Actions.SetNumber(), CompLevel: string(submit2024Actions.CompLevel()),
StartingQuadrant: 0, SpeakerAuto: 0, AmpAuto: 0, NotesDroppedAuto: 0, MobilityAuto: false,
Speaker: 0, Amp: 0, SpeakerAmplified: 0, AmpAmplified: 0, NotesDropped: 0, Penalties: 0,
- TrapNote: false, AvgCycle: 0, Park: false, OnStage: false, Harmony: false, CollectedBy: "",
+ TrapNote: false, Spotlight: false, AvgCycle: 0, Park: false, OnStage: false, Harmony: false, CollectedBy: "",
}
// Loop over all actions.
for i := 0; i < submit2024Actions.ActionsListLength(); i++ {
@@ -537,6 +537,7 @@
stat.Harmony = true
}
stat.TrapNote = endMatchAction.TrapNote()
+ stat.Spotlight = endMatchAction.Spotlight()
}
}
if cycles != 0 {
@@ -589,6 +590,7 @@
NotesDropped: stat.NotesDropped,
Penalties: stat.Penalties,
TrapNote: stat.TrapNote,
+ Spotlight: stat.Spotlight,
AvgCycle: stat.AvgCycle,
Park: stat.Park,
OnStage: stat.OnStage,
diff --git a/scouting/webserver/requests/requests_test.go b/scouting/webserver/requests/requests_test.go
index eb5e904..ebe73f3 100644
--- a/scouting/webserver/requests/requests_test.go
+++ b/scouting/webserver/requests/requests_test.go
@@ -137,7 +137,7 @@
MatchNumber: 1, SetNumber: 1, CompLevel: "qm", StartingQuadrant: 3,
SpeakerAuto: 2, AmpAuto: 4, NotesDroppedAuto: 1, MobilityAuto: true,
Speaker: 0, Amp: 1, SpeakerAmplified: 2, AmpAmplified: 1,
- NotesDropped: 0, Penalties: 01, TrapNote: true, AvgCycle: 233,
+ NotesDropped: 0, Penalties: 01, TrapNote: true, Spotlight: false, AvgCycle: 233,
Park: false, OnStage: true, Harmony: false, CollectedBy: "alex",
},
{
@@ -145,7 +145,7 @@
MatchNumber: 3, SetNumber: 1, CompLevel: "qm", StartingQuadrant: 1,
SpeakerAuto: 0, AmpAuto: 2, NotesDroppedAuto: 0, MobilityAuto: false,
Speaker: 0, Amp: 4, SpeakerAmplified: 3, AmpAmplified: 1,
- NotesDropped: 0, Penalties: 1, TrapNote: true, AvgCycle: 120,
+ NotesDropped: 0, Penalties: 1, TrapNote: true, Spotlight: false, AvgCycle: 120,
Park: true, OnStage: false, Harmony: false, CollectedBy: "bob",
},
},
@@ -214,7 +214,7 @@
MatchNumber: 3, SetNumber: 1, CompLevel: "quals", StartingQuadrant: 4,
SpeakerAuto: 1, AmpAuto: 1, NotesDroppedAuto: 0, MobilityAuto: true,
Speaker: 4, Amp: 2, SpeakerAmplified: 1, AmpAmplified: 0,
- NotesDropped: 2, Penalties: 2, TrapNote: true, AvgCycle: 0,
+ NotesDropped: 2, Penalties: 2, TrapNote: true, Spotlight: true, AvgCycle: 0,
Park: true, OnStage: false, Harmony: false, CollectedBy: "alex",
},
{
@@ -222,7 +222,7 @@
MatchNumber: 3, SetNumber: 1, CompLevel: "quals", StartingQuadrant: 2,
SpeakerAuto: 0, AmpAuto: 0, NotesDroppedAuto: 0, MobilityAuto: false,
Speaker: 0, Amp: 2, SpeakerAmplified: 3, AmpAmplified: 2,
- NotesDropped: 1, Penalties: 0, TrapNote: false, AvgCycle: 0,
+ NotesDropped: 1, Penalties: 0, TrapNote: false, Spotlight: true, AvgCycle: 0,
Park: false, OnStage: true, Harmony: false, CollectedBy: "george",
},
},
@@ -247,7 +247,7 @@
MatchNumber: 3, SetNumber: 1, CompLevel: "quals", StartingQuadrant: 4,
SpeakerAuto: 1, AmpAuto: 1, NotesDroppedAuto: 0, MobilityAuto: true,
Speaker: 4, Amp: 2, SpeakerAmplified: 1, AmpAmplified: 0,
- NotesDropped: 2, Penalties: 2, TrapNote: true, AvgCycle: 0,
+ NotesDropped: 2, Penalties: 2, TrapNote: true, Spotlight: true, AvgCycle: 0,
Park: true, OnStage: false, Harmony: false, CollectedBy: "alex",
},
{
@@ -255,7 +255,7 @@
MatchNumber: 3, SetNumber: 1, CompLevel: "quals", StartingQuadrant: 2,
SpeakerAuto: 0, AmpAuto: 0, NotesDroppedAuto: 0, MobilityAuto: false,
Speaker: 0, Amp: 2, SpeakerAmplified: 3, AmpAmplified: 2,
- NotesDropped: 1, Penalties: 0, TrapNote: false, AvgCycle: 0,
+ NotesDropped: 1, Penalties: 0, TrapNote: false, Spotlight: true, AvgCycle: 0,
Park: false, OnStage: true, Harmony: false, CollectedBy: "george",
},
},
@@ -459,6 +459,7 @@
Value: &submit_2024_actions.EndMatchActionT{
StageType: submit_2024_actions.StageTypekHARMONY,
TrapNote: false,
+ Spotlight: false,
},
},
Timestamp: 4200,
@@ -479,7 +480,7 @@
MatchNumber: 3, SetNumber: 1, CompLevel: "quals", StartingQuadrant: 2,
SpeakerAuto: 0, AmpAuto: 1, NotesDroppedAuto: 1, MobilityAuto: true,
Speaker: 0, Amp: 0, SpeakerAmplified: 1, AmpAmplified: 1,
- NotesDropped: 0, Penalties: 5, TrapNote: false, AvgCycle: 950,
+ NotesDropped: 0, Penalties: 5, TrapNote: false, Spotlight: false, AvgCycle: 950,
Park: false, OnStage: false, Harmony: true, CollectedBy: "",
}
@@ -1200,7 +1201,7 @@
MatchNumber: 2, SetNumber: 1, CompLevel: "quals", StartingQuadrant: 0,
SpeakerAuto: 0, AmpAuto: 0, NotesDroppedAuto: 0, MobilityAuto: false,
Speaker: 1, Amp: 0, SpeakerAmplified: 0, AmpAmplified: 0,
- NotesDropped: 0, Penalties: 0, TrapNote: false, AvgCycle: 0,
+ NotesDropped: 0, Penalties: 0, TrapNote: false, Spotlight: false, AvgCycle: 0,
Park: false, OnStage: false, Harmony: false, CollectedBy: "debug_cli",
},
}
@@ -1446,7 +1447,7 @@
MatchNumber: 3, SetNumber: 1, CompLevel: "quals", StartingQuadrant: 2,
SpeakerAuto: 0, AmpAuto: 1, NotesDroppedAuto: 1, MobilityAuto: true,
Speaker: 0, Amp: 1, SpeakerAmplified: 1, AmpAmplified: 1,
- NotesDropped: 0, Penalties: 1, TrapNote: true, AvgCycle: 233,
+ NotesDropped: 0, Penalties: 1, TrapNote: true, Spotlight: false, AvgCycle: 233,
Park: false, OnStage: false, Harmony: true, CollectedBy: "alek",
},
{
@@ -1454,7 +1455,7 @@
MatchNumber: 5, SetNumber: 3, CompLevel: "quals", StartingQuadrant: 1,
SpeakerAuto: 0, AmpAuto: 0, NotesDroppedAuto: 0, MobilityAuto: false,
Speaker: 0, Amp: 0, SpeakerAmplified: 3, AmpAmplified: 1,
- NotesDropped: 0, Penalties: 1, TrapNote: false, AvgCycle: 120,
+ NotesDropped: 0, Penalties: 1, TrapNote: false, Spotlight: false, AvgCycle: 120,
Park: false, OnStage: true, Harmony: false, CollectedBy: "kacey",
},
},
@@ -1518,7 +1519,7 @@
MatchNumber: 5, SetNumber: 3, CompLevel: "quals", StartingQuadrant: 1,
SpeakerAuto: 0, AmpAuto: 0, NotesDroppedAuto: 0, MobilityAuto: false,
Speaker: 0, Amp: 0, SpeakerAmplified: 3, AmpAmplified: 1,
- NotesDropped: 0, Penalties: 1, TrapNote: false, AvgCycle: 120,
+ NotesDropped: 0, Penalties: 1, TrapNote: false, Spotlight: false, AvgCycle: 120,
Park: false, OnStage: true, Harmony: false, CollectedBy: "kacey",
},
}
diff --git a/scouting/www/entry/entry.component.css b/scouting/www/entry/entry.component.css
index 246887c..9c50ee0 100644
--- a/scouting/www/entry/entry.component.css
+++ b/scouting/www/entry/entry.component.css
@@ -18,3 +18,7 @@
input label {
padding: 0px;
}
+
+#EndGame > div > label {
+ padding: 0;
+}
diff --git a/scouting/www/entry/entry.component.ts b/scouting/www/entry/entry.component.ts
index b3e1e92..122c415 100644
--- a/scouting/www/entry/entry.component.ts
+++ b/scouting/www/entry/entry.component.ts
@@ -86,6 +86,7 @@
type: 'endMatchAction';
stageType: StageType;
trapNote: boolean;
+ spotlight: boolean;
timestamp?: number;
}
| {
@@ -112,6 +113,7 @@
readonly COMP_LEVELS = COMP_LEVELS;
readonly COMP_LEVEL_LABELS = COMP_LEVEL_LABELS;
readonly ScoreType = ScoreType;
+ readonly StageType = StageType;
section: Section = 'Team Selection';
@Input() matchNumber: number = 1;
@@ -374,7 +376,8 @@
const endMatchActionOffset = EndMatchAction.createEndMatchAction(
builder,
action.stageType,
- action.trapNote
+ action.trapNote,
+ action.spotlight
);
actionOffset = Action.createAction(
builder,
diff --git a/scouting/www/entry/entry.ng.html b/scouting/www/entry/entry.ng.html
index 9490237..ea44ad3 100644
--- a/scouting/www/entry/entry.ng.html
+++ b/scouting/www/entry/entry.ng.html
@@ -294,7 +294,7 @@
<h6 class="text-muted">
Last Action: {{actionList[actionList.length - 1].type}}
</h6>
- <div class="d-grid gap-4">
+ <div class="d-grid gap-2">
<button class="btn btn-secondary" (click)="undoLastAction()">UNDO</button>
<button
class="btn btn-danger"
@@ -302,7 +302,7 @@
>
DEAD
</button>
- <label style="padding: 0">
+ <label>
<input
#park
type="radio"
@@ -312,7 +312,7 @@
/>
Park
</label>
- <label style="padding: 0">
+ <label>
<input
#onStage
type="radio"
@@ -322,7 +322,7 @@
/>
On Stage
</label>
- <label style="padding: 0">
+ <label>
<input
#harmony
type="radio"
@@ -332,7 +332,7 @@
/>
Harmony
</label>
- <label style="padding: 0">
+ <label>
<input
#trapNote
type="checkbox"
@@ -342,6 +342,16 @@
/>
Trap Note
</label>
+ <label>
+ <input
+ #spotlight
+ type="checkbox"
+ id="spotlight"
+ name="spotlight"
+ value="spotlight"
+ />
+ Spotlight
+ </label>
<div style="display: flex">
<h5>Penalties :</h5>
<button
@@ -363,7 +373,7 @@
<button
*ngIf="!autoPhase"
class="btn btn-info"
- (click)="changeSectionTo('Review and Submit'); addPenalties(); addAction({type: 'endMatchAction', park: park.checked, onStage: onStage.checked, harmony: harmony.checked, trapNote: trapNote.checked});"
+ (click)="changeSectionTo('Review and Submit'); addPenalties(); addAction({type: 'endMatchAction', stageType: (park.checked ? StageType.kPARK : onStage.checked ? StageType.kON_STAGE : harmony.checked ? StageType.kHARMONY : StageType.kMISSING), trapNote: trapNote.checked, spotlight: spotlight.checked});"
>
End Match
</button>
@@ -398,8 +408,10 @@
</span>
<span *ngSwitchCase="'endAutoPhase'">Ended auto phase</span>
<span *ngSwitchCase="'endMatchAction'">
- Ended Match; park: {{action.park}}, onStage: {{action.onStage}},
- harmony: {{action.harmony}}, trapNote: {{action.trapNote}}
+ Ended Match; stageType: {{(action.stageType === 0 ? "kON_STAGE" :
+ action.stageType === 1 ? "kPARK" : action.stageType === 2 ?
+ "kHARMONY" : "kMISSING")}}, trapNote: {{action.trapNote}},
+ spotlight: {{action.spotlight}}
</span>
<span *ngSwitchCase="'robotDeathAction'">
Robot on: {{action.robotOn}}
diff --git a/y2024/BUILD b/y2024/BUILD
index 0e8c31f..52386e8 100644
--- a/y2024/BUILD
+++ b/y2024/BUILD
@@ -19,7 +19,9 @@
":aos_config",
"//aos/starter:roborio_irq_config.json",
"//y2024/constants:constants.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 = [
@@ -71,6 +73,8 @@
],
start_binaries = [
"//aos/events/logging:logger_main",
+ "//frc971/imu_fdcan:can_translator",
+ "//frc971/imu_fdcan:dual_imu_blender",
"//aos/network:message_bridge_client",
"//aos/network:message_bridge_server",
"//aos/network:web_proxy_main",
@@ -111,7 +115,11 @@
flatbuffers = [
"//aos/network:message_bridge_client_fbs",
"//aos/network:message_bridge_server_fbs",
+ "//frc971/imu_fdcan:dual_imu_fbs",
+ "//frc971/imu_fdcan:can_translator_status_fbs",
+ "//frc971/imu_fdcan:dual_imu_blender_status_fbs",
"//y2024/constants:constants_fbs",
+ "//frc971/control_loops/drivetrain/localization:localizer_output_fbs",
"//frc971/can_logger:can_logging_fbs",
"//aos/network:timestamp_fbs",
"//aos/network:remote_message_fbs",
@@ -132,6 +140,7 @@
"//aos/network:remote_message_fbs",
"//aos/network:message_bridge_client_fbs",
"//aos/network:message_bridge_server_fbs",
+ "//frc971/wpilib:pdp_values_fbs",
#y2019 stuff shouldn't be here (e.g. target selector)
"//y2024/constants:constants_fbs",
"//aos/network:timestamp_fbs",
diff --git a/y2024/control_loops/drivetrain/drivetrain_main.cc b/y2024/control_loops/drivetrain/drivetrain_main.cc
index e0b2eb7..82629de 100644
--- a/y2024/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2024/control_loops/drivetrain/drivetrain_main.cc
@@ -18,14 +18,15 @@
frc971::constants::WaitForConstants<y2024::Constants>(&config.message());
aos::ShmEventLoop event_loop(&config.message());
+ const auto drivetrain_config =
+ ::y2024::control_loops::drivetrain::GetDrivetrainConfig(&event_loop);
+
std::unique_ptr<::frc971::control_loops::drivetrain::PuppetLocalizer>
localizer = std::make_unique<
::frc971::control_loops::drivetrain::PuppetLocalizer>(
- &event_loop,
- ::y2024::control_loops::drivetrain::GetDrivetrainConfig(&event_loop));
+ &event_loop, drivetrain_config);
std::unique_ptr<DrivetrainLoop> drivetrain = std::make_unique<DrivetrainLoop>(
- y2024::control_loops::drivetrain::GetDrivetrainConfig(&event_loop),
- &event_loop, localizer.get());
+ drivetrain_config, &event_loop, localizer.get());
event_loop.Run();
diff --git a/y2024/vision/maps/target_map.json b/y2024/vision/maps/target_map.json
index a87e9a3..2a8dfef 100644
--- a/y2024/vision/maps/target_map.json
+++ b/y2024/vision/maps/target_map.json
@@ -1,41 +1,46 @@
{
-/* Targets have positive Z pointing out of the board, positive X to the left and
- positive Y is down. This means that the camera lines up with the target if the
- board is upright and the camera is facing away from the board.*/
+/* Targets have positive Z axis pointing into the board, positive X to the right
+ when looking at the board, and positive Y is down when looking at the board.
+ This means that you will get an identity rotation from the camera to target
+ frame when the target is upright, flat, and centered in the camera's view.
+
+ The global frame as the origin at the center of the field, positive X points
+ at the red driver's station, and positive Z points straight up.
+ */
"target_poses": [
{
"id": 1,
"position": {
- "x": -6.809,
- "y": 3.860,
+ "x": 6.809,
+ "y": -3.860,
"z": 1.361
},
"orientation": {
- "w": -0.18,
- "x": -0.68,
- "y": 0.68,
- "z": 0.18
+ "w": 0.1830127,
+ "x": -0.1830127,
+ "y": 0.6830127,
+ "z": -0.6830127
}
},
{
"id": 2,
"position": {
- "x": -7.915,
- "y": 3.223,
+ "x": 7.915,
+ "y": -3.223,
"z": 1.361
},
"orientation": {
- "w": -0.18,
- "x": -0.68,
- "y": 0.68,
- "z": 0.18
+ "w": 0.1830127,
+ "x": -0.1830127,
+ "y": 0.6830127,
+ "z": -0.6830127
}
},
{
"id": 3,
"position": {
- "x": -8.309,
- "y": -0.877,
+ "x": 8.309,
+ "y": 0.877,
"z": 1.456
},
"orientation": {
@@ -48,8 +53,8 @@
{
"id": 4,
"position": {
- "x": -8.309,
- "y": -1.442,
+ "x": 8.309,
+ "y": 1.442,
"z": 1.456
},
"orientation": {
@@ -62,120 +67,120 @@
{
"id": 5,
"position": {
- "x": -6.428,
- "y": -4.099,
+ "x": 6.428,
+ "y": 4.099,
"z": 1.361
},
"orientation": {
- "w": 0.707,
- "x": 0.0,
- "y": -0.707,
+ "w": 0.7071068,
+ "x": -0.7071068,
+ "y": 0.0,
"z": 0.0
}
},
{
"id": 6,
"position": {
- "x": 6.430,
- "y": -4.099,
+ "x": -6.430,
+ "y": 4.099,
"z": 1.361
},
"orientation": {
- "w": 0.707,
- "x": 0.0,
- "y": -0.707,
+ "w": 0.7071068,
+ "x": -0.7071068,
+ "y": 0.0,
"z": 0.0
}
},
{
"id": 7,
"position": {
- "x": 8.309,
- "y": -1.442,
+ "x": -8.309,
+ "y": 1.442,
"z": 1.474
},
"orientation": {
"w": 0.5,
- "x": 0.5,
+ "x": -0.5,
"y": -0.5,
- "z": -0.5
+ "z": 0.5
}
},
{
"id": 8,
"position": {
- "x": 8.309,
- "y": -0.877,
+ "x": -8.309,
+ "y": 0.877,
"z": 1.474
},
"orientation": {
"w": 0.5,
- "x": 0.5,
+ "x": -0.5,
"y": -0.5,
- "z": -0.5
+ "z": 0.5
}
},
{
"id": 9,
"position": {
- "x": 7.915,
- "y": 3.223,
+ "x": -7.915,
+ "y": -3.223,
"z": 1.361
},
"orientation": {
- "w": -0.18,
- "x": -0.68,
- "y": 0.68,
- "z": 0.18
+ "w": 0.1830127,
+ "x": -0.1830127,
+ "y": -0.6830127,
+ "z": 0.6830127
}
},
{
"id": 10,
"position": {
- "x": 6.809,
- "y": 3.860,
+ "x": -6.809,
+ "y": -3.860,
"z": 1.361
},
"orientation": {
- "w": -0.18,
- "x": -0.68,
- "y": 0.68,
- "z": 0.18
+ "w": 0.1830127,
+ "x": -0.1830127,
+ "y": -0.6830127,
+ "z": 0.6830127
}
},
{
"id": 11,
"position": {
- "x": -3.629,
- "y": 0.393,
+ "x": 3.629,
+ "y": -0.393,
"z": 1.326
},
"orientation": {
- "w": 0.68,
- "x": 0.18,
- "y": -0.18,
- "z": -0.68
+ "w": 0.6830127,
+ "x": -0.6830127,
+ "y": -0.1830127,
+ "z": 0.1830127
}
},
{
"id": 12,
"position": {
- "x": -3.630,
- "y": -0.392,
+ "x": 3.630,
+ "y": 0.392,
"z": 1.326
},
"orientation": {
- "w": -0.18,
- "x": -0.68,
- "y": 0.68,
- "z": 0.18
+ "w": 0.1830127,
+ "x": -0.1830127,
+ "y": -0.6830127,
+ "z": 0.6830127
}
},
{
"id": 13,
"position": {
- "x": -2.949,
- "y": 0.000,
+ "x": 2.949,
+ "y": -0.000,
"z": 1.326
},
"orientation": {
@@ -188,43 +193,43 @@
{
"id": 14,
"position": {
- "x": 2.949,
- "y": 0.000,
+ "x": -2.949,
+ "y": -0.000,
"z": 1.326
},
"orientation": {
"w": 0.5,
- "x": 0.5,
+ "x": -0.5,
"y": -0.5,
- "z": -0.5
+ "z": 0.5
}
},
{
"id": 15,
"position": {
- "x": 3.629,
- "y": -0.393,
+ "x": -3.629,
+ "y": 0.393,
"z": 1.326
},
"orientation": {
- "w": -0.18,
- "x": -0.68,
- "y": 0.68,
- "z": 0.18
+ "w": 0.1830127,
+ "x": -0.1830127,
+ "y": 0.6830127,
+ "z": -0.6830127
}
},
{
"id": 16,
"position": {
- "x": 3.630,
- "y": 0.392,
+ "x": -3.630,
+ "y": -0.392,
"z": 1.326
},
"orientation": {
- "w": 0.68,
- "x": 0.18,
- "y": -0.18,
- "z": -0.68
+ "w": 0.6830127,
+ "x": -0.6830127,
+ "y": 0.1830127,
+ "z": -0.1830127
}
}
]
diff --git a/y2024/wpilib_interface.cc b/y2024/wpilib_interface.cc
index bce5451..6e75387 100644
--- a/y2024/wpilib_interface.cc
+++ b/y2024/wpilib_interface.cc
@@ -135,8 +135,15 @@
::frc971::control_loops::drivetrain::PositionStatic>(
"/drivetrain")),
gyro_sender_(event_loop->MakeSender<::frc971::sensors::GyroReading>(
- "/drivetrain")){};
- void Start() override { AddToDMA(&imu_yaw_rate_reader_); }
+ "/drivetrain")) {
+ UpdateFastEncoderFilterHz(kMaxFastEncoderPulsesPerSecond);
+ event_loop->SetRuntimeAffinity(aos::MakeCpusetFromCpus({0}));
+ };
+ void Start() override {
+ AddToDMA(&imu_yaw_rate_reader_);
+ AddToDMA(&turret_encoder_.reader());
+ AddToDMA(&altitude_encoder_.reader());
+ }
// Auto mode switches.
void set_autonomous_mode(int i, ::std::unique_ptr<frc::DigitalInput> sensor) {
@@ -325,9 +332,46 @@
frc971::wpilib::AbsoluteEncoder intake_pivot_encoder_;
frc971::wpilib::AbsoluteEncoderAndPotentiometer climber_encoder_,
- catapult_encoder_, turret_encoder_, altitude_encoder_, extend_encoder_;
+ catapult_encoder_, extend_encoder_;
frc971::wpilib::DMAPulseWidthReader imu_yaw_rate_reader_;
+
+ frc971::wpilib::DMAAbsoluteEncoderAndPotentiometer turret_encoder_,
+ altitude_encoder_;
+};
+
+class SuperstructurePWMWriter
+ : public ::frc971::wpilib::LoopOutputHandler<superstructure::Output> {
+ public:
+ SuperstructurePWMWriter(aos::EventLoop *event_loop)
+ : frc971::wpilib::LoopOutputHandler<superstructure::Output>(
+ event_loop, "/superstructure") {}
+
+ void set_catapult_kraken_one(::std::unique_ptr<::frc::TalonFX> t) {
+ catapult_kraken_one_ = ::std::move(t);
+ }
+ void set_catapult_kraken_two(::std::unique_ptr<::frc::TalonFX> t) {
+ catapult_kraken_two_ = ::std::move(t);
+ }
+
+ private:
+ void Stop() override {
+ AOS_LOG(WARNING, "Superstructure output too old.\n");
+ catapult_kraken_one_->SetDisabled();
+ catapult_kraken_two_->SetDisabled();
+ }
+
+ void Write(const superstructure::Output &output) override {
+ WritePwm(output.catapult_voltage(), catapult_kraken_one_.get());
+ WritePwm(output.catapult_voltage(), catapult_kraken_one_.get());
+ }
+
+ template <typename T>
+ static void WritePwm(const double voltage, T *motor) {
+ motor->SetSpeed(std::clamp(voltage, -kMaxBringupPower, kMaxBringupPower) /
+ 12.0);
+ }
+ ::std::unique_ptr<::frc::TalonFX> catapult_kraken_one_, catapult_kraken_two_;
};
class WPILibRobot : public ::frc971::wpilib::WPILibRobotBase {
@@ -401,7 +445,7 @@
robot_constants->common()->current_limits();
std::shared_ptr<TalonFX> right_front = std::make_shared<TalonFX>(
- 0, false, "Drivetrain Bus", &canivore_signal_registry,
+ 2, false, "Drivetrain Bus", &canivore_signal_registry,
current_limits->drivetrain_supply_current_limit(),
current_limits->drivetrain_stator_current_limit());
std::shared_ptr<TalonFX> right_back = std::make_shared<TalonFX>(
@@ -409,35 +453,36 @@
current_limits->drivetrain_supply_current_limit(),
current_limits->drivetrain_stator_current_limit());
std::shared_ptr<TalonFX> left_front = std::make_shared<TalonFX>(
- 2, false, "Drivetrain Bus", &canivore_signal_registry,
+ 4, false, "Drivetrain Bus", &canivore_signal_registry,
current_limits->drivetrain_supply_current_limit(),
current_limits->drivetrain_stator_current_limit());
std::shared_ptr<TalonFX> left_back = std::make_shared<TalonFX>(
- 3, false, "Drivetrain Bus", &canivore_signal_registry,
+ 5, false, "Drivetrain Bus", &canivore_signal_registry,
current_limits->drivetrain_supply_current_limit(),
current_limits->drivetrain_stator_current_limit());
std::shared_ptr<TalonFX> intake_pivot = std::make_shared<TalonFX>(
- 4, false, "Drivetrain Bus", &canivore_signal_registry,
+ 3, false, "Drivetrain Bus", &canivore_signal_registry,
current_limits->intake_pivot_stator_current_limit(),
current_limits->intake_pivot_supply_current_limit());
+ // TODO(max): Assign these proper ids
std::shared_ptr<TalonFX> altitude = std::make_shared<TalonFX>(
- 5, false, "Drivetrain Bus", &canivore_signal_registry,
+ 6, false, "Drivetrain Bus", &canivore_signal_registry,
current_limits->altitude_stator_current_limit(),
current_limits->altitude_supply_current_limit());
std::shared_ptr<TalonFX> turret = std::make_shared<TalonFX>(
- 6, false, "Drivetrain Bus", &canivore_signal_registry,
+ 7, false, "Drivetrain Bus", &canivore_signal_registry,
current_limits->turret_stator_current_limit(),
current_limits->turret_supply_current_limit());
std::shared_ptr<TalonFX> intake_roller = std::make_shared<TalonFX>(
- 7, false, "rio", &rio_signal_registry,
+ 8, false, "rio", &rio_signal_registry,
current_limits->intake_roller_stator_current_limit(),
current_limits->intake_roller_supply_current_limit());
std::shared_ptr<TalonFX> transfer_roller = std::make_shared<TalonFX>(
- 8, false, "rio", &rio_signal_registry,
+ 9, false, "rio", &rio_signal_registry,
current_limits->transfer_roller_stator_current_limit(),
current_limits->transfer_roller_supply_current_limit());
std::shared_ptr<TalonFX> climber = std::make_shared<TalonFX>(
- 9, false, "rio", &rio_signal_registry,
+ 10, false, "rio", &rio_signal_registry,
current_limits->climber_stator_current_limit(),
current_limits->climber_supply_current_limit());
@@ -504,9 +549,7 @@
drivetrain_can_position_sender.MakeStaticBuilder();
auto drivetrain_falcon_vector =
- drivetrain_can_builder->add_talonfxs();
-
- CHECK(drivetrain_falcon_vector->reserve(drivetrain_talonfxs.size()));
+ CHECK_NOTNULL(drivetrain_can_builder->add_talonfxs());
for (auto talonfx : drivetrain_talonfxs) {
talonfx->SerializePosition(
@@ -631,6 +674,14 @@
AddLoop(&can_output_event_loop);
+ ::aos::ShmEventLoop pwm_event_loop(&config.message());
+ SuperstructurePWMWriter superstructure_pwm_writer(&pwm_event_loop);
+ superstructure_pwm_writer.set_catapult_kraken_one(
+ make_unique<frc::TalonFX>(0));
+ superstructure_pwm_writer.set_catapult_kraken_one(
+ make_unique<frc::TalonFX>(1));
+
+ AddLoop(&pwm_event_loop);
// Thread 6
RunLoops();
diff --git a/y2024/www/field.html b/y2024/www/field.html
index 8c3b291..ac94060 100644
--- a/y2024/www/field.html
+++ b/y2024/www/field.html
@@ -81,6 +81,22 @@
<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> 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>
</table>
</body>
</html>
diff --git a/y2024/www/field_handler.ts b/y2024/www/field_handler.ts
index f383c08..81050dd 100644
--- a/y2024/www/field_handler.ts
+++ b/y2024/www/field_handler.ts
@@ -21,11 +21,75 @@
export class FieldHandler {
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 x: HTMLElement = (document.getElementById('x') as HTMLElement);
+ private y: HTMLElement = (document.getElementById('y') as HTMLElement);
+ private theta: HTMLElement =
+ (document.getElementById('theta') as HTMLElement);
+
private fieldImage: HTMLImageElement = new Image();
+
+ 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 falconLeftFrontPosition: HTMLElement =
+ (document.getElementById('falcon_left_front') as HTMLElement);
+ private falconLeftBackPosition: HTMLElement =
+ (document.getElementById('falcon_left_back') as HTMLElement);
+
constructor(private readonly connection: Connection) {
(document.getElementById('field') as HTMLElement).appendChild(this.canvas);
this.fieldImage.src = '2024.png';
+
+ this.connection.addConfigHandler(() => {
+
+ this.connection.addHandler(
+ '/drivetrain', 'frc971.control_loops.drivetrain.Status', (data) => {
+ this.handleDrivetrainStatus(data);
+ });
+ this.connection.addHandler(
+ '/drivetrain', 'frc971.control_loops.drivetrain.Position', (data) => {
+ this.handleDrivetrainPosition(data);
+ });
+ this.connection.addHandler(
+ '/drivetrain', 'frc971.control_loops.drivetrain.CANPosition', (data) => {
+ this.handleDrivetrainCANPosition(data);
+ });
+ this.connection.addHandler(
+ '/localizer', 'frc971.controls.LocalizerOutput', (data) => {
+ this.handleLocalizerOutput(data);
+ });
+ });
+ }
+
+ private handleDrivetrainStatus(data: Uint8Array): void {
+ const fbBuffer = new ByteBuffer(data);
+ 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 handleLocalizerOutput(data: Uint8Array): void {
+ const fbBuffer = new ByteBuffer(data);
+ this.localizerOutput = LocalizerOutput.getRootAsLocalizerOutput(fbBuffer);
}
drawField(): void {
@@ -38,10 +102,95 @@
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();
+}
+
+ 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');
+}
draw(): void {
this.reset();
this.drawField();
+ if (this.drivetrainPosition) {
+ this.leftDrivetrainEncoder.innerHTML =
+ this.drivetrainPosition.leftEncoder().toString();
+
+ this.rightDrivetrainEncoder.innerHTML =
+ this.drivetrainPosition.rightEncoder().toString();
+ }
+
+ if (this.drivetrainCANPosition) {
+ this.falconRightFrontPosition.innerHTML =
+ this.drivetrainCANPosition.talonfxs(0).position().toString();
+
+ this.falconRightBackPosition.innerHTML =
+ this.drivetrainCANPosition.talonfxs(1).position().toString();
+
+ this.falconLeftFrontPosition.innerHTML =
+ this.drivetrainCANPosition.talonfxs(2).position().toString();
+
+ this.falconLeftBackPosition.innerHTML =
+ this.drivetrainCANPosition.talonfxs(3).position().toString();
+ }
+
+ 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());
}
diff --git a/y2024/y2024_imu.json b/y2024/y2024_imu.json
index 862a1d6..e22e010 100644
--- a/y2024/y2024_imu.json
+++ b/y2024/y2024_imu.json
@@ -150,6 +150,36 @@
}
]
},
+ {
+ "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
+ },
{
"name": "/roborio/aos/remote_timestamps/imu/roborio/aos/aos-starter-StarterRpc",
"type": "aos.message_bridge.RemoteMessage",
@@ -160,6 +190,22 @@
"max_size": 200
},
{
+ "name": "/imu",
+ "type": "frc971.imu.DualImu",
+ "source_node": "imu",
+ "frequency": 1100,
+ "num_senders": 1,
+ "max_size": 496
+ },
+ {
+ "name": "/imu",
+ "type": "frc971.imu.CanTranslatorStatus",
+ "source_node": "imu",
+ "frequency": 1000,
+ "num_senders": 1,
+ "max_size": 200
+ },
+ {
"name": "/can/cana",
"type": "frc971.can_logger.CanFrame",
"source_node": "imu",
@@ -192,6 +238,14 @@
"num_senders": 2
},
{
+ "name": "/imu",
+ "type": "frc971.imu.DualImuBlenderStatus",
+ "source_node": "imu",
+ "frequency": 1100,
+ "num_senders": 1,
+ "max_size": 200
+ },
+ {
"name": "/imu/constants",
"type": "y2024.Constants",
"source_node": "imu",
@@ -252,6 +306,24 @@
"imu"
]
},
+ // TODO(max): Update the channel value with whatever channel the IMU is on.
+ {
+ "name": "can_translator",
+ "executable_name": "can_translator",
+ "args": [
+ "--channel=/can/canb"
+ ],
+ "nodes": [
+ "imu"
+ ]
+ },
+ {
+ "name": "dual_imu_blender",
+ "executable_name": "dual_imu_blender",
+ "nodes": [
+ "imu"
+ ]
+ },
{
"name": "web_proxy",
"executable_name": "web_proxy_main",
diff --git a/y2024/y2024_roborio.json b/y2024/y2024_roborio.json
index 6af86f0..b78daef 100644
--- a/y2024/y2024_roborio.json
+++ b/y2024/y2024_roborio.json
@@ -19,6 +19,13 @@
},
{
"name": "/roborio/aos",
+ "type": "frc971.PDPValues",
+ "source_node": "roborio",
+ "frequency": 55,
+ "max_size": 368
+ },
+ {
+ "name": "/roborio/aos",
"type": "aos.RobotState",
"source_node": "roborio",
"frequency": 250
@@ -144,7 +151,7 @@
"source_node": "roborio",
"frequency": 220,
"num_senders": 2,
- "max_size": 400
+ "max_size": 608
},
{
"name": "/superstructure/rio",
@@ -152,7 +159,7 @@
"source_node": "roborio",
"frequency": 220,
"num_senders": 2,
- "max_size": 400
+ "max_size": 608
},
{
"name": "/can",
@@ -168,7 +175,7 @@
"source_node": "roborio",
"frequency": 220,
"num_senders": 2,
- "max_size": 400
+ "max_size": 424
},
{
"name": "/drivetrain",
@@ -213,7 +220,7 @@
"type": "frc971.control_loops.drivetrain.Position",
"source_node": "roborio",
"frequency": 400,
- "max_size": 112,
+ "max_size": 128,
"num_senders": 2
},
{