Add y2024 folder
2023 bot specific code was removed.
Signed-off-by: Nathan Leong <100028864@mvla.net>
Change-Id: I88fc4a4b5e6bc883ea327cc306efa4e20035908b
diff --git a/y2024/BUILD b/y2024/BUILD
new file mode 100644
index 0000000..010b17c
--- /dev/null
+++ b/y2024/BUILD
@@ -0,0 +1,267 @@
+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:aos_config",
+)
+
+robot_downloader(
+ binaries = [
+ "//aos/network:web_proxy_main",
+ "//aos/events/logging:log_cat",
+ "//y2024/constants:constants_sender",
+ "//aos/events:aos_timing_report_streamer",
+ ],
+ data = [
+ ":aos_config",
+ "//aos/starter:roborio_irq_config.json",
+ "//y2024/constants:constants.json",
+ "@ctre_phoenix6_tools_athena//:shared_libraries",
+ "@ctre_phoenix_cci_athena//:shared_libraries",
+ ],
+ dirs = [
+ "//y2024/www:www_files",
+ "//y2024/autonomous:splines",
+ ],
+ start_binaries = [
+ "//aos/events/logging:logger_main",
+ "//aos/network:web_proxy_main",
+ "//aos/starter:irq_affinity",
+ "//y2024/autonomous:binaries",
+ ":joystick_reader",
+ ":wpilib_interface",
+ "//frc971/can_logger",
+ "//aos/network:message_bridge_client",
+ "//aos/network:message_bridge_server",
+ "//y2024/control_loops/drivetrain:drivetrain",
+ "//y2024/control_loops/drivetrain:trajectory_generator",
+ "//y2024/control_loops/superstructure:superstructure",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+)
+
+robot_downloader(
+ name = "orin_download",
+ binaries = [
+ "//aos/starter:irq_affinity",
+ "//aos/util:foxglove_websocket",
+ "//aos/events:aos_timing_report_streamer",
+ "//y2024/constants:constants_sender",
+ "//aos/network:web_proxy_main",
+ ":joystick_republish",
+ "//aos/events/logging:log_cat",
+ "//frc971/image_streamer:image_streamer",
+ ],
+ data = [
+ ":aos_config",
+ "//y2024/constants:constants.json",
+ "//y2024/vision:image_streamer_start",
+ "//y2024/www:www_files",
+ ],
+ dirs = [
+ "//y2024/www:www_files",
+ "//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",
+ "//y2024/vision:image_logger",
+ "//aos/events/logging:logger_main",
+ ],
+ target_compatible_with = ["//tools/platforms/hardware:raspberry_pi"],
+ target_type = "orin",
+)
+
+aos_config(
+ name = "aos_config",
+ src = "y2024.json",
+ flatbuffers = [
+ "//aos/network:message_bridge_client_fbs",
+ "//aos/network:message_bridge_server_fbs",
+ "//aos/network:timestamp_fbs",
+ "//frc971/input:robot_state_fbs",
+ "//frc971/vision:vision_fbs",
+ "//frc971/vision:target_map_fbs",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":config_imu",
+ ":config_roborio",
+ ],
+)
+
+aos_config(
+ name = "config_imu",
+ src = "y2024_imu.json",
+ flatbuffers = [
+ "//aos/network:message_bridge_client_fbs",
+ "//aos/network:message_bridge_server_fbs",
+ "//y2024/constants:constants_fbs",
+ "//aos/network:timestamp_fbs",
+ "//aos/network:remote_message_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_roborio.json",
+ flatbuffers = [
+ "//frc971:can_configuration_fbs",
+ "//aos/network:remote_message_fbs",
+ "//aos/network:message_bridge_client_fbs",
+ "//aos/network:message_bridge_server_fbs",
+ #y2019 stuff shouldn't be here (e.g. target selector)
+ "//y2024/constants:constants_fbs",
+ "//aos/network:timestamp_fbs",
+ "//y2024/control_loops/superstructure:superstructure_goal_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_can_position_fbs",
+ "//y2024/control_loops/superstructure:superstructure_output_fbs",
+ "//y2024/control_loops/superstructure:superstructure_position_fbs",
+ "//y2024/control_loops/superstructure:superstructure_status_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",
+ ],
+)
+
+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/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:dma",
+ "//frc971/wpilib:drivetrain_writer",
+ "//frc971/wpilib:encoder_and_potentiometer",
+ "//frc971/wpilib:joystick_sender",
+ "//frc971/wpilib:logging_fbs",
+ "//frc971/wpilib:pdp_fetcher",
+ "//frc971/wpilib:sensor_reader",
+ "//frc971/wpilib:wpilib_robot_base",
+ "//third_party:phoenix",
+ "//third_party:phoenix6",
+ "//third_party:wpilib",
+ "//y2024/control_loops/superstructure:superstructure_output_fbs",
+ "//y2024/control_loops/superstructure:superstructure_position_fbs",
+ ],
+)
+
+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/control_loops/drivetrain:drivetrain_base",
+ "//y2024/control_loops/superstructure:superstructure_goal_fbs",
+ "//y2024/control_loops/superstructure:superstructure_status_fbs",
+ ],
+)
+
+cc_binary(
+ name = "joystick_republish",
+ srcs = [
+ "joystick_republish.cc",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos:configuration",
+ "//aos:flatbuffer_merge",
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ "//frc971/input:joystick_state_fbs",
+ "@com_github_google_glog//:glog",
+ ],
+)
+
+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/www:files",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+)
diff --git a/y2024/__init__.py b/y2024/__init__.py
new file mode 100644
index 0000000..e69de29
--- /dev/null
+++ b/y2024/__init__.py
diff --git a/y2024/autonomous/BUILD b/y2024/autonomous/BUILD
new file mode 100644
index 0000000..611f3cd
--- /dev/null
+++ b/y2024/autonomous/BUILD
@@ -0,0 +1,74 @@
+load("//frc971/downloader:downloader.bzl", "aos_downloader_dir")
+
+filegroup(
+ name = "binaries.stripped",
+ srcs = [
+ ":autonomous_action.stripped",
+ ],
+ visibility = ["//visibility:public"],
+)
+
+filegroup(
+ name = "binaries",
+ srcs = [
+ ":autonomous_action",
+ ],
+ visibility = ["//visibility:public"],
+)
+
+filegroup(
+ name = "spline_jsons",
+ srcs = glob([
+ "splines/*.json",
+ ]),
+ visibility = ["//visibility:public"],
+)
+
+aos_downloader_dir(
+ name = "splines",
+ srcs = [
+ ":spline_jsons",
+ ],
+ dir = "splines",
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+)
+
+cc_library(
+ name = "autonomous_action_lib",
+ srcs = [
+ "auto_splines.cc",
+ "autonomous_actor.cc",
+ ],
+ hdrs = [
+ "auto_splines.h",
+ "autonomous_actor.h",
+ ],
+ deps = [
+ "//aos/events:event_loop",
+ "//aos/logging",
+ "//aos/util:phased_loop",
+ "//frc971/autonomous:base_autonomous_actor",
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops:profiled_subsystem_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_config",
+ "//frc971/control_loops/drivetrain:localizer_fbs",
+ "//y2024:constants",
+ "//y2024/control_loops/drivetrain:drivetrain_base",
+ "//y2024/control_loops/superstructure:superstructure_goal_fbs",
+ "//y2024/control_loops/superstructure:superstructure_status_fbs",
+ ],
+)
+
+cc_binary(
+ name = "autonomous_action",
+ srcs = [
+ "autonomous_actor_main.cc",
+ ],
+ deps = [
+ ":autonomous_action_lib",
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ "//frc971/autonomous:auto_fbs",
+ ],
+)
diff --git a/y2024/autonomous/auto_splines.cc b/y2024/autonomous/auto_splines.cc
new file mode 100644
index 0000000..d2bd3a1
--- /dev/null
+++ b/y2024/autonomous/auto_splines.cc
@@ -0,0 +1,126 @@
+#include "y2024/autonomous/auto_splines.h"
+
+#include "aos/flatbuffer_merge.h"
+#include "frc971/control_loops/control_loops_generated.h"
+
+namespace y2024 {
+namespace autonomous {
+
+namespace {
+flatbuffers::Offset<frc971::MultiSpline> FixSpline(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ flatbuffers::Offset<frc971::MultiSpline> spline_offset,
+ aos::Alliance alliance) {
+ frc971::MultiSpline *spline =
+ GetMutableTemporaryPointer(*builder->fbb(), spline_offset);
+ flatbuffers::Vector<float> *spline_x = spline->mutable_spline_x();
+
+ // For 2024: The field is mirrored across the center line, and is not
+ // rotationally symmetric. As such, we only flip the X coordinates when
+ // changing side of the field.
+ if (alliance == aos::Alliance::kBlue) {
+ for (size_t ii = 0; ii < spline_x->size(); ++ii) {
+ spline_x->Mutate(ii, -spline_x->Get(ii));
+ }
+ }
+ return spline_offset;
+}
+} // namespace
+
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::BasicSSpline(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance) {
+ flatbuffers::Offset<frc971::Constraint> longitudinal_constraint_offset;
+ flatbuffers::Offset<frc971::Constraint> lateral_constraint_offset;
+ flatbuffers::Offset<frc971::Constraint> voltage_constraint_offset;
+
+ {
+ frc971::Constraint::Builder longitudinal_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ longitudinal_constraint_builder.add_constraint_type(
+ frc971::ConstraintType::LONGITUDINAL_ACCELERATION);
+ longitudinal_constraint_builder.add_value(1.0);
+ longitudinal_constraint_offset = longitudinal_constraint_builder.Finish();
+ }
+
+ {
+ frc971::Constraint::Builder lateral_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ lateral_constraint_builder.add_constraint_type(
+ frc971::ConstraintType::LATERAL_ACCELERATION);
+ lateral_constraint_builder.add_value(1.0);
+ lateral_constraint_offset = lateral_constraint_builder.Finish();
+ }
+
+ {
+ frc971::Constraint::Builder voltage_constraint_builder =
+ builder->MakeBuilder<frc971::Constraint>();
+ voltage_constraint_builder.add_constraint_type(
+ frc971::ConstraintType::VOLTAGE);
+ voltage_constraint_builder.add_value(6.0);
+ voltage_constraint_offset = voltage_constraint_builder.Finish();
+ }
+
+ flatbuffers::Offset<
+ flatbuffers::Vector<flatbuffers::Offset<frc971::Constraint>>>
+ constraints_offset =
+ builder->fbb()->CreateVector<flatbuffers::Offset<frc971::Constraint>>(
+ {longitudinal_constraint_offset, lateral_constraint_offset,
+ voltage_constraint_offset});
+
+ const float startx = 0.4;
+ const float starty = 3.4;
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder->fbb()->CreateVector<float>({0.0f + startx, 0.6f + startx,
+ 0.6f + startx, 0.4f + startx,
+ 0.4f + startx, 1.0f + startx});
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder->fbb()->CreateVector<float>({starty - 0.0f, starty - 0.0f,
+ starty - 0.3f, starty - 0.7f,
+ starty - 1.0f, starty - 1.0f});
+
+ frc971::MultiSpline::Builder multispline_builder =
+ builder->MakeBuilder<frc971::MultiSpline>();
+
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_constraints(constraints_offset);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ return FixSpline(builder, multispline_builder.Finish(), alliance);
+}
+
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::TestSpline(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance) {
+ return FixSpline(
+ builder,
+ aos::CopyFlatBuffer<frc971::MultiSpline>(test_spline_, builder->fbb()),
+ alliance);
+}
+
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::StraightLine(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance) {
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
+ builder->fbb()->CreateVector<float>(
+ {-12.3, -11.9, -11.5, -11.1, -10.6, -10.0});
+ flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset =
+ builder->fbb()->CreateVector<float>({1.25, 1.25, 1.25, 1.25, 1.25, 1.25});
+
+ frc971::MultiSpline::Builder multispline_builder =
+ builder->MakeBuilder<frc971::MultiSpline>();
+
+ multispline_builder.add_spline_count(1);
+ multispline_builder.add_spline_x(spline_x_offset);
+ multispline_builder.add_spline_y(spline_y_offset);
+
+ return FixSpline(builder, multispline_builder.Finish(), alliance);
+}
+
+} // namespace autonomous
+} // namespace y2024
diff --git a/y2024/autonomous/auto_splines.h b/y2024/autonomous/auto_splines.h
new file mode 100644
index 0000000..efdfb33
--- /dev/null
+++ b/y2024/autonomous/auto_splines.h
@@ -0,0 +1,45 @@
+#ifndef Y2024_AUTONOMOUS_AUTO_SPLINES_H_
+#define Y2024_AUTONOMOUS_AUTO_SPLINES_H_
+
+#include "aos/events/event_loop.h"
+#include "aos/flatbuffer_merge.h"
+#include "frc971/control_loops/control_loops_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
+#include "frc971/input/joystick_state_generated.h"
+/*
+
+ The cooridinate system for the autonomous splines is the same as the spline
+ python generator and drivetrain spline systems.
+
+*/
+
+namespace y2024 {
+namespace autonomous {
+
+class AutonomousSplines {
+ public:
+ AutonomousSplines()
+ : test_spline_(aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
+ "splines/test_spline.json")) {}
+ static flatbuffers::Offset<frc971::MultiSpline> BasicSSpline(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance);
+ static flatbuffers::Offset<frc971::MultiSpline> StraightLine(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance);
+
+ flatbuffers::Offset<frc971::MultiSpline> TestSpline(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance);
+
+ private:
+ aos::FlatbufferDetachedBuffer<frc971::MultiSpline> test_spline_;
+};
+
+} // namespace autonomous
+} // namespace y2024
+
+#endif // Y2024_AUTONOMOUS_AUTO_SPLINES_H_
diff --git a/y2024/autonomous/autonomous_actor.cc b/y2024/autonomous/autonomous_actor.cc
new file mode 100644
index 0000000..fd42a56
--- /dev/null
+++ b/y2024/autonomous/autonomous_actor.cc
@@ -0,0 +1,196 @@
+#include "y2024/autonomous/autonomous_actor.h"
+
+#include <chrono>
+#include <cinttypes>
+#include <cmath>
+
+#include "aos/logging/logging.h"
+#include "aos/util/math.h"
+#include "frc971/control_loops/drivetrain/localizer_generated.h"
+#include "y2024/autonomous/auto_splines.h"
+#include "y2024/constants.h"
+#include "y2024/control_loops/drivetrain/drivetrain_base.h"
+
+DEFINE_bool(spline_auto, false, "Run simple test S-spline auto mode.");
+
+namespace y2024 {
+namespace autonomous {
+
+using ::frc971::ProfileParametersT;
+
+ProfileParametersT MakeProfileParameters(float max_velocity,
+ float max_acceleration) {
+ ProfileParametersT result;
+ result.max_velocity = max_velocity;
+ result.max_acceleration = max_acceleration;
+ return result;
+}
+
+using ::aos::monotonic_clock;
+using frc971::CreateProfileParameters;
+using ::frc971::ProfileParametersT;
+using frc971::control_loops::CreateStaticZeroingSingleDOFProfiledSubsystemGoal;
+using frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal;
+using frc971::control_loops::drivetrain::LocalizerControl;
+namespace chrono = ::std::chrono;
+
+AutonomousActor::AutonomousActor(::aos::EventLoop *event_loop)
+ : frc971::autonomous::BaseAutonomousActor(
+ event_loop, control_loops::drivetrain::GetDrivetrainConfig()),
+ localizer_control_sender_(
+ event_loop->MakeSender<
+ ::frc971::control_loops::drivetrain::LocalizerControl>(
+ "/drivetrain")),
+ joystick_state_fetcher_(
+ event_loop->MakeFetcher<aos::JoystickState>("/aos")),
+ robot_state_fetcher_(event_loop->MakeFetcher<aos::RobotState>("/aos")),
+ auto_splines_(),
+ superstructure_goal_sender_(
+ event_loop->MakeSender<::y2024::control_loops::superstructure::Goal>(
+ "/superstructure")),
+ superstructure_status_fetcher_(
+ event_loop
+ ->MakeFetcher<::y2024::control_loops::superstructure::Status>(
+ "/superstructure")) {
+ drivetrain_status_fetcher_.Fetch();
+ replan_timer_ = event_loop->AddTimer([this]() { Replan(); });
+
+ event_loop->OnRun([this, event_loop]() {
+ replan_timer_->Schedule(event_loop->monotonic_now());
+ button_poll_->Schedule(event_loop->monotonic_now(),
+ chrono::milliseconds(50));
+ });
+
+ // TODO(james): Really need to refactor this code since we keep using it.
+ button_poll_ = event_loop->AddTimer([this]() {
+ const aos::monotonic_clock::time_point now =
+ this->event_loop()->context().monotonic_event_time;
+ if (robot_state_fetcher_.Fetch()) {
+ if (robot_state_fetcher_->user_button()) {
+ user_indicated_safe_to_reset_ = true;
+ MaybeSendStartingPosition();
+ }
+ }
+ if (joystick_state_fetcher_.Fetch()) {
+ if (joystick_state_fetcher_->has_alliance() &&
+ (joystick_state_fetcher_->alliance() != alliance_)) {
+ alliance_ = joystick_state_fetcher_->alliance();
+ is_planned_ = false;
+ // Only kick the planning out by 2 seconds. If we end up enabled in
+ // that second, then we will kick it out further based on the code
+ // below.
+ replan_timer_->Schedule(now + std::chrono::seconds(2));
+ }
+ if (joystick_state_fetcher_->enabled()) {
+ if (!is_planned_) {
+ // Only replan once we've been disabled for 5 seconds.
+ replan_timer_->Schedule(now + std::chrono::seconds(5));
+ }
+ }
+ }
+ });
+}
+
+void AutonomousActor::Replan() {
+ if (!drivetrain_status_fetcher_.Fetch()) {
+ replan_timer_->Schedule(event_loop()->monotonic_now() + chrono::seconds(1));
+ AOS_LOG(INFO, "Drivetrain not up, replanning in 1 second");
+ return;
+ }
+
+ if (alliance_ == aos::Alliance::kInvalid) {
+ return;
+ }
+ sent_starting_position_ = false;
+ if (FLAGS_spline_auto) {
+ test_spline_ =
+ PlanSpline(std::bind(&AutonomousSplines::TestSpline, &auto_splines_,
+ std::placeholders::_1, alliance_),
+ SplineDirection::kForward);
+
+ starting_position_ = test_spline_->starting_position();
+ }
+
+ is_planned_ = true;
+
+ MaybeSendStartingPosition();
+}
+
+void AutonomousActor::MaybeSendStartingPosition() {
+ if (is_planned_ && user_indicated_safe_to_reset_ &&
+ !sent_starting_position_) {
+ CHECK(starting_position_);
+ SendStartingPosition(starting_position_.value());
+ }
+}
+
+void AutonomousActor::Reset() {
+ InitializeEncoders();
+ ResetDrivetrain();
+
+ joystick_state_fetcher_.Fetch();
+ CHECK(joystick_state_fetcher_.get() != nullptr)
+ << "Expect at least one JoystickState message before running auto...";
+ alliance_ = joystick_state_fetcher_->alliance();
+ preloaded_ = false;
+}
+
+bool AutonomousActor::RunAction(
+ const ::frc971::autonomous::AutonomousActionParams *params) {
+ Reset();
+
+ AOS_LOG(INFO, "Params are %d\n", params->mode());
+
+ if (!user_indicated_safe_to_reset_) {
+ AOS_LOG(WARNING, "Didn't send starting position prior to starting auto.");
+ CHECK(starting_position_);
+ SendStartingPosition(starting_position_.value());
+ }
+ // Clear this so that we don't accidentally resend things as soon as we
+ // replan later.
+ user_indicated_safe_to_reset_ = false;
+ is_planned_ = false;
+ starting_position_.reset();
+
+ AOS_LOG(INFO, "Params are %d\n", params->mode());
+ if (alliance_ == aos::Alliance::kInvalid) {
+ AOS_LOG(INFO, "Aborting autonomous due to invalid alliance selection.");
+ return false;
+ }
+ if (FLAGS_spline_auto) {
+ SplineAuto();
+ } else {
+ AOS_LOG(WARNING, "No auto mode selected.");
+ }
+ return true;
+}
+
+void AutonomousActor::SplineAuto() {
+ CHECK(test_spline_);
+
+ if (!test_spline_->WaitForPlan()) return;
+ test_spline_->Start();
+
+ if (!test_spline_->WaitForSplineDistanceRemaining(0.02)) return;
+}
+
+void AutonomousActor::SendStartingPosition(const Eigen::Vector3d &start) {
+ // Set up the starting position for the blue alliance.
+
+ auto builder = localizer_control_sender_.MakeBuilder();
+
+ LocalizerControl::Builder localizer_control_builder =
+ builder.MakeBuilder<LocalizerControl>();
+ localizer_control_builder.add_x(start(0));
+ localizer_control_builder.add_y(start(1));
+ localizer_control_builder.add_theta(start(2));
+ localizer_control_builder.add_theta_uncertainty(0.00001);
+ AOS_LOG(INFO, "User button pressed, x: %f y: %f theta: %f", start(0),
+ start(1), start(2));
+ if (builder.Send(localizer_control_builder.Finish()) !=
+ aos::RawSender::Error::kOk) {
+ AOS_LOG(ERROR, "Failed to reset localizer.\n");
+ }
+}
+} // namespace autonomous
+} // namespace y2024
diff --git a/y2024/autonomous/autonomous_actor.h b/y2024/autonomous/autonomous_actor.h
new file mode 100644
index 0000000..6603080
--- /dev/null
+++ b/y2024/autonomous/autonomous_actor.h
@@ -0,0 +1,66 @@
+#ifndef Y2024_AUTONOMOUS_AUTONOMOUS_ACTOR_H_
+#define Y2024_AUTONOMOUS_AUTONOMOUS_ACTOR_H_
+
+#include "aos/actions/actions.h"
+#include "aos/actions/actor.h"
+#include "frc971/autonomous/base_autonomous_actor.h"
+#include "frc971/control_loops/control_loops_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/drivetrain/localizer_generated.h"
+#include "y2024/autonomous/auto_splines.h"
+#include "y2024/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2024/control_loops/superstructure/superstructure_status_generated.h"
+
+namespace y2024 {
+namespace autonomous {
+
+class AutonomousActor : public ::frc971::autonomous::BaseAutonomousActor {
+ public:
+ explicit AutonomousActor(::aos::EventLoop *event_loop);
+
+ bool RunAction(
+ const ::frc971::autonomous::AutonomousActionParams *params) override;
+
+ private:
+ void SendSuperstructureGoal();
+
+ void Reset();
+
+ void SendStartingPosition(const Eigen::Vector3d &start);
+ void MaybeSendStartingPosition();
+ void SplineAuto();
+ void Replan();
+
+ aos::Sender<frc971::control_loops::drivetrain::LocalizerControl>
+ localizer_control_sender_;
+ aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
+ aos::Fetcher<aos::RobotState> robot_state_fetcher_;
+
+ aos::TimerHandler *replan_timer_;
+ aos::TimerHandler *button_poll_;
+
+ aos::Alliance alliance_ = aos::Alliance::kInvalid;
+ AutonomousSplines auto_splines_;
+ bool user_indicated_safe_to_reset_ = false;
+ bool sent_starting_position_ = false;
+
+ bool is_planned_ = false;
+
+ std::optional<Eigen::Vector3d> starting_position_;
+
+ bool preloaded_ = false;
+
+ aos::Sender<control_loops::superstructure::Goal> superstructure_goal_sender_;
+ aos::Fetcher<y2024::control_loops::superstructure::Status>
+ superstructure_status_fetcher_;
+
+ std::optional<SplineHandle> test_spline_;
+
+ // List of arm angles from arm::PointsList
+ const ::std::vector<::Eigen::Matrix<double, 3, 1>> points_;
+};
+
+} // namespace autonomous
+} // namespace y2024
+
+#endif // Y2024_AUTONOMOUS_AUTONOMOUS_ACTOR_H_
diff --git a/y2024/autonomous/autonomous_actor_main.cc b/y2024/autonomous/autonomous_actor_main.cc
new file mode 100644
index 0000000..4a8e146
--- /dev/null
+++ b/y2024/autonomous/autonomous_actor_main.cc
@@ -0,0 +1,19 @@
+#include <cstdio>
+
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "y2024/autonomous/autonomous_actor.h"
+
+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::autonomous::AutonomousActor autonomous(&event_loop);
+
+ event_loop.Run();
+
+ return 0;
+}
diff --git a/y2024/constants.cc b/y2024/constants.cc
new file mode 100644
index 0000000..cdaf28c
--- /dev/null
+++ b/y2024/constants.cc
@@ -0,0 +1,50 @@
+#include "y2024/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 {
+namespace constants {
+
+Values MakeValues(uint16_t team) {
+ LOG(INFO) << "creating a Constants for team: " << team;
+
+ Values r;
+
+ switch (team) {
+ // A set of constants for tests.
+ case 1:
+ break;
+
+ case kCompTeamNumber:
+ break;
+
+ case kPracticeTeamNumber:
+ break;
+
+ case kCodingRobotTeamNumber:
+ break;
+
+ default:
+ LOG(FATAL) << "unknown team: " << team;
+
+ // TODO(milind): add pot range checks once we add ranges
+ }
+
+ return r;
+}
+
+Values MakeValues() { return MakeValues(aos::network::GetTeamNumber()); }
+
+} // namespace constants
+} // namespace y2024
diff --git a/y2024/constants.h b/y2024/constants.h
new file mode 100644
index 0000000..b9e5a7d
--- /dev/null
+++ b/y2024/constants.h
@@ -0,0 +1,70 @@
+#ifndef Y2024_CONSTANTS_H_
+#define Y2024_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/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+
+namespace y2024 {
+namespace constants {
+
+constexpr uint16_t kCompTeamNumber = 971;
+constexpr uint16_t kPracticeTeamNumber = 9971;
+constexpr uint16_t kCodingRobotTeamNumber = 7971;
+
+struct Values {
+ static const int kZeroingSampleSize = 200;
+
+ static const int kSuperstructureCANWriterPriority = 35;
+ static const int kDrivetrainWriterPriority = 35;
+ static const int kDrivetrainTxPriority = 36;
+ static const int kDrivetrainRxPriority = 36;
+
+ // TODO: These values will need to be changed for the 2024 robot.
+ 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.
+constants::Values MakeValues(uint16_t team);
+
+// Calls MakeValues with aos::network::GetTeamNumber()
+constants::Values MakeValues();
+
+} // namespace constants
+} // namespace y2024
+
+#endif // Y2024_CONSTANTS_H_
diff --git a/y2024/constants/7971.json b/y2024/constants/7971.json
new file mode 100644
index 0000000..94fc375
--- /dev/null
+++ b/y2024/constants/7971.json
@@ -0,0 +1,5 @@
+{
+ "cameras": [
+ ],
+ {% include 'y2024/constants/common.json' %}
+}
diff --git a/y2024/constants/971.json b/y2024/constants/971.json
new file mode 100644
index 0000000..2e6ffa8
--- /dev/null
+++ b/y2024/constants/971.json
@@ -0,0 +1,8 @@
+{
+ "cameras": [
+ ],
+ "robot": {
+ }
+ },
+ {% include 'y2024/constants/common.json' %}
+}
diff --git a/y2024/constants/9971.json b/y2024/constants/9971.json
new file mode 100644
index 0000000..2e6ffa8
--- /dev/null
+++ b/y2024/constants/9971.json
@@ -0,0 +1,8 @@
+{
+ "cameras": [
+ ],
+ "robot": {
+ }
+ },
+ {% include 'y2024/constants/common.json' %}
+}
diff --git a/y2024/constants/BUILD b/y2024/constants/BUILD
new file mode 100644
index 0000000..1e8faa4
--- /dev/null
+++ b/y2024/constants/BUILD
@@ -0,0 +1,78 @@
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
+load("//tools/build_rules:template.bzl", "jinja2_template")
+
+cc_library(
+ name = "simulated_constants_sender",
+ srcs = ["simulated_constants_sender.cc"],
+ hdrs = ["simulated_constants_sender.h"],
+ data = [":test_constants.json"],
+ visibility = ["//y2024:__subpackages__"],
+ deps = [
+ ":constants_fbs",
+ ":constants_list_fbs",
+ "//aos/events:simulated_event_loop",
+ "//aos/testing:path",
+ "//frc971/constants:constants_sender_lib",
+ ],
+)
+
+jinja2_template(
+ name = "test_constants.json",
+ src = "test_constants.jinja2.json",
+ includes = glob(["test_data/*.json"]),
+ parameters = {},
+ visibility = ["//visibility:public"],
+)
+
+jinja2_template(
+ name = "constants.json",
+ src = "constants.jinja2.json",
+ includes = [
+ "7971.json",
+ "971.json",
+ "9971.json",
+ "common.json",
+ ":target_map",
+ "//y2024/vision/maps",
+ ],
+ parameters = {},
+ visibility = ["//visibility:public"],
+)
+
+filegroup(
+ name = "target_map",
+ srcs = [
+ "target_map.json",
+ ],
+ visibility = ["//y2024:__subpackages__"],
+)
+
+static_flatbuffer(
+ name = "constants_fbs",
+ srcs = ["constants.fbs"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//frc971/vision:target_map_fbs",
+ ],
+)
+
+static_flatbuffer(
+ name = "constants_list_fbs",
+ srcs = ["constants_list.fbs"],
+ visibility = ["//visibility:public"],
+ deps = [":constants_fbs"],
+)
+
+cc_binary(
+ name = "constants_sender",
+ srcs = ["constants_sender.cc"],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":constants_fbs",
+ ":constants_list_fbs",
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ "//aos/testing:path",
+ "//frc971/constants:constants_sender_lib",
+ ],
+)
diff --git a/y2024/constants/common.json b/y2024/constants/common.json
new file mode 100644
index 0000000..15894ad
--- /dev/null
+++ b/y2024/constants/common.json
@@ -0,0 +1,5 @@
+ "target_map": {% include 'y2024/constants/target_map.json' %},
+ "ignore_targets": {
+ "red": [4],
+ "blue": [5]
+ }
diff --git a/y2024/constants/constants.fbs b/y2024/constants/constants.fbs
new file mode 100644
index 0000000..ad34617
--- /dev/null
+++ b/y2024/constants/constants.fbs
@@ -0,0 +1,15 @@
+include "frc971/vision/target_map.fbs";
+
+namespace y2024;
+
+
+table RobotConstants {
+
+}
+
+table Constants {
+ target_map:frc971.vision.TargetMap (id: 0);
+ robot:RobotConstants (id: 1);
+}
+
+root_type Constants;
diff --git a/y2024/constants/constants.jinja2.json b/y2024/constants/constants.jinja2.json
new file mode 100644
index 0000000..c27e588
--- /dev/null
+++ b/y2024/constants/constants.jinja2.json
@@ -0,0 +1,16 @@
+{
+ "constants": [
+ {
+ "team": 7971,
+ "data": {% include 'y2024/constants/7971.json' %}
+ },
+ {
+ "team": 971,
+ "data": {% include 'y2024/constants/971.json' %}
+ },
+ {
+ "team": 9971,
+ "data": {% include 'y2024/constants/9971.json' %}
+ }
+ ]
+}
diff --git a/y2024/constants/constants_list.fbs b/y2024/constants/constants_list.fbs
new file mode 100644
index 0000000..dbebc19
--- /dev/null
+++ b/y2024/constants/constants_list.fbs
@@ -0,0 +1,14 @@
+include "y2024/constants/constants.fbs";
+
+namespace y2024;
+
+table TeamAndConstants {
+ team:long (id: 0);
+ data:Constants (id: 1);
+}
+
+table ConstantsList {
+ constants:[TeamAndConstants] (id: 0);
+}
+
+root_type ConstantsList;
diff --git a/y2024/constants/constants_sender.cc b/y2024/constants/constants_sender.cc
new file mode 100644
index 0000000..bc442a2
--- /dev/null
+++ b/y2024/constants/constants_sender.cc
@@ -0,0 +1,24 @@
+#include "gflags/gflags.h"
+#include "glog/logging.h"
+
+#include "aos/configuration.h"
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "aos/json_to_flatbuffer.h"
+#include "frc971/constants/constants_sender_lib.h"
+#include "y2024/constants/constants_generated.h"
+#include "y2024/constants/constants_list_generated.h"
+
+DEFINE_string(config, "aos_config.json", "Path to the AOS config.");
+DEFINE_string(constants_path, "constants.json", "Path to the constant file");
+
+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::constants::ConstantSender<y2024::Constants, y2024::ConstantsList>
+ constants_sender(&event_loop, FLAGS_constants_path);
+ // Don't need to call Run().
+ return 0;
+}
diff --git a/y2024/constants/simulated_constants_sender.cc b/y2024/constants/simulated_constants_sender.cc
new file mode 100644
index 0000000..28189f8
--- /dev/null
+++ b/y2024/constants/simulated_constants_sender.cc
@@ -0,0 +1,18 @@
+#include "aos/events/simulated_event_loop.h"
+#include "aos/testing/path.h"
+#include "frc971/constants/constants_sender_lib.h"
+#include "y2024/constants/constants_generated.h"
+#include "y2024/constants/constants_list_generated.h"
+
+namespace y2024 {
+bool SendSimulationConstants(aos::SimulatedEventLoopFactory *factory, int team,
+ std::string constants_path) {
+ for (const aos::Node *node : factory->nodes()) {
+ std::unique_ptr<aos::EventLoop> event_loop =
+ factory->MakeEventLoop("constants_sender", node);
+ frc971::constants::ConstantSender<Constants, ConstantsList> sender(
+ event_loop.get(), constants_path, team, "/constants");
+ }
+ return true;
+}
+} // namespace y2024
diff --git a/y2024/constants/simulated_constants_sender.h b/y2024/constants/simulated_constants_sender.h
new file mode 100644
index 0000000..5ecc4da
--- /dev/null
+++ b/y2024/constants/simulated_constants_sender.h
@@ -0,0 +1,16 @@
+#ifndef Y2024_CONSTANTS_SIMULATED_CONFIG_SENDER_H_
+#define Y2024_CONSTANTS_SIMULATED_CONFIG_SENDER_H_
+
+#include "aos/events/simulated_event_loop.h"
+#include "aos/testing/path.h"
+
+namespace y2024 {
+// Returns true, to allow this to be easily called in the initializer list of a
+// constructor.
+bool SendSimulationConstants(
+ aos::SimulatedEventLoopFactory *factory, int team,
+ std::string constants_path =
+ aos::testing::ArtifactPath("y2024/constants/test_constants.json"));
+} // namespace y2024
+
+#endif // Y2024_CONSTANTS_SIMULATED_CONFIG_SENDER_H_
diff --git a/y2024/constants/target_map.json b/y2024/constants/target_map.json
new file mode 100644
index 0000000..544b7b4
--- /dev/null
+++ b/y2024/constants/target_map.json
@@ -0,0 +1,3 @@
+{
+
+}
\ No newline at end of file
diff --git a/y2024/constants/test_constants.jinja2.json b/y2024/constants/test_constants.jinja2.json
new file mode 100644
index 0000000..c6b77dd
--- /dev/null
+++ b/y2024/constants/test_constants.jinja2.json
@@ -0,0 +1,8 @@
+{
+ "constants": [
+ {
+ "team": 7971,
+ "data": {}
+ }
+ ]
+}
diff --git a/y2024/control_loops/BUILD b/y2024/control_loops/BUILD
new file mode 100644
index 0000000..1747d2d
--- /dev/null
+++ b/y2024/control_loops/BUILD
@@ -0,0 +1,6 @@
+py_library(
+ name = "python_init",
+ srcs = ["__init__.py"],
+ visibility = ["//visibility:public"],
+ deps = ["//y2024:python_init"],
+)
diff --git a/y2024/control_loops/__init__.py b/y2024/control_loops/__init__.py
new file mode 100644
index 0000000..e69de29
--- /dev/null
+++ b/y2024/control_loops/__init__.py
diff --git a/y2024/control_loops/drivetrain/BUILD b/y2024/control_loops/drivetrain/BUILD
new file mode 100644
index 0000000..e3c627a
--- /dev/null
+++ b/y2024/control_loops/drivetrain/BUILD
@@ -0,0 +1,116 @@
+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/control_loops/python:drivetrain) $(OUTS)",
+ target_compatible_with = ["@platforms//os:linux"],
+ tools = [
+ "//y2024/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/control_loops/python:polydrivetrain) $(OUTS)",
+ target_compatible_with = ["@platforms//os:linux"],
+ tools = [
+ "//y2024/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/constants:constants_sender_lib",
+ "//frc971/control_loops/drivetrain:drivetrain_lib",
+ "//frc971/control_loops/drivetrain/localization:puppet_localizer",
+ "//y2024/constants:constants_fbs",
+ ],
+)
+
+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:aos_config",
+ ],
+)
+
+cc_binary(
+ name = "trajectory_generator",
+ srcs = [
+ "trajectory_generator_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:trajectory_generator",
+ ],
+)
diff --git a/y2024/control_loops/drivetrain/drivetrain_base.cc b/y2024/control_loops/drivetrain/drivetrain_base.cc
new file mode 100644
index 0000000..88b74f9
--- /dev/null
+++ b/y2024/control_loops/drivetrain/drivetrain_base.cc
@@ -0,0 +1,90 @@
+#include "y2024/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/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "y2024/control_loops/drivetrain/hybrid_velocity_drivetrain.h"
+#include "y2024/control_loops/drivetrain/kalman_drivetrain_motor_plant.h"
+#include "y2024/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 {
+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 */,
+ true /*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
diff --git a/y2024/control_loops/drivetrain/drivetrain_base.h b/y2024/control_loops/drivetrain/drivetrain_base.h
new file mode 100644
index 0000000..807981a
--- /dev/null
+++ b/y2024/control_loops/drivetrain/drivetrain_base.h
@@ -0,0 +1,17 @@
+#ifndef Y2024_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_BASE_H_
+#define Y2024_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_BASE_H_
+
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+
+namespace y2024 {
+namespace control_loops {
+namespace drivetrain {
+
+const ::frc971::control_loops::drivetrain::DrivetrainConfig<double> &
+GetDrivetrainConfig();
+
+} // namespace drivetrain
+} // namespace control_loops
+} // namespace y2024
+
+#endif // Y2024_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_BASE_H_
diff --git a/y2024/control_loops/drivetrain/drivetrain_main.cc b/y2024/control_loops/drivetrain/drivetrain_main.cc
new file mode 100644
index 0000000..196fbb7
--- /dev/null
+++ b/y2024/control_loops/drivetrain/drivetrain_main.cc
@@ -0,0 +1,33 @@
+#include <memory>
+
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "frc971/constants/constants_sender_lib.h"
+#include "frc971/control_loops/drivetrain/drivetrain.h"
+#include "frc971/control_loops/drivetrain/localization/puppet_localizer.h"
+#include "y2024/constants/constants_generated.h"
+#include "y2024/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");
+
+ frc971::constants::WaitForConstants<y2024::Constants>(&config.message());
+
+ 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::control_loops::drivetrain::GetDrivetrainConfig());
+ std::unique_ptr<DrivetrainLoop> drivetrain = std::make_unique<DrivetrainLoop>(
+ y2024::control_loops::drivetrain::GetDrivetrainConfig(), &event_loop,
+ localizer.get());
+
+ event_loop.Run();
+
+ return 0;
+}
diff --git a/y2024/control_loops/drivetrain/drivetrain_simulation_config.json b/y2024/control_loops/drivetrain/drivetrain_simulation_config.json
new file mode 100644
index 0000000..f0886b0
--- /dev/null
+++ b/y2024/control_loops/drivetrain/drivetrain_simulation_config.json
@@ -0,0 +1,6 @@
+{
+ "imports": [
+ "../../y2024.json",
+ "../../../frc971/control_loops/drivetrain/drivetrain_simulation_channels.json"
+ ]
+}
diff --git a/y2024/control_loops/drivetrain/trajectory_generator_main.cc b/y2024/control_loops/drivetrain/trajectory_generator_main.cc
new file mode 100644
index 0000000..260f4a3
--- /dev/null
+++ b/y2024/control_loops/drivetrain/trajectory_generator_main.cc
@@ -0,0 +1,39 @@
+#include <sys/resource.h>
+#include <sys/time.h>
+
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "frc971/control_loops/drivetrain/trajectory_generator.h"
+#include "y2024/control_loops/drivetrain/drivetrain_base.h"
+
+using ::frc971::control_loops::drivetrain::TrajectoryGenerator;
+
+DEFINE_bool(skip_renicing, false,
+ "If true, skip renicing the trajectory generator.");
+
+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());
+ TrajectoryGenerator generator(
+ &event_loop, ::y2024::control_loops::drivetrain::GetDrivetrainConfig());
+
+ event_loop.OnRun([]() {
+ if (FLAGS_skip_renicing) {
+ LOG(WARNING) << "Ignoring request to renice to -20 due to "
+ "--skip_renicing.";
+ } else {
+ errno = 0;
+ setpriority(PRIO_PROCESS, 0, -20);
+ PCHECK(errno == 0)
+ << ": Renicing to -20 failed, use --skip_renicing to skip renicing.";
+ }
+ });
+
+ event_loop.Run();
+
+ return 0;
+}
diff --git a/y2024/control_loops/python/BUILD b/y2024/control_loops/python/BUILD
new file mode 100644
index 0000000..4451fb9
--- /dev/null
+++ b/y2024/control_loops/python/BUILD
@@ -0,0 +1,53 @@
+package(default_visibility = ["//y2024:__subpackages__"])
+
+py_binary(
+ name = "drivetrain",
+ srcs = [
+ "drivetrain.py",
+ ],
+ legacy_create_init = False,
+ 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,
+ deps = [
+ ":python_init",
+ "//frc971/control_loops/python:polydrivetrain",
+ "@pip//glog",
+ "@pip//python_gflags",
+ ],
+)
+
+py_library(
+ name = "polydrivetrain_lib",
+ srcs = [
+ "drivetrain.py",
+ "polydrivetrain.py",
+ ],
+ 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"],
+ visibility = ["//visibility:public"],
+ deps = ["//y2024/control_loops:python_init"],
+)
diff --git a/y2024/control_loops/python/__init__.py b/y2024/control_loops/python/__init__.py
new file mode 100644
index 0000000..e69de29
--- /dev/null
+++ b/y2024/control_loops/python/__init__.py
diff --git a/y2024/control_loops/python/drivetrain.py b/y2024/control_loops/python/drivetrain.py
new file mode 100644
index 0000000..1d1054a
--- /dev/null
+++ b/y2024/control_loops/python/drivetrain.py
@@ -0,0 +1,47 @@
+#!/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', kDrivetrain)
+
+
+if __name__ == '__main__':
+ sys.exit(main(sys.argv))
diff --git a/y2024/control_loops/python/polydrivetrain.py b/y2024/control_loops/python/polydrivetrain.py
new file mode 100644
index 0000000..99a9b53
--- /dev/null
+++ b/y2024/control_loops/python/polydrivetrain.py
@@ -0,0 +1,33 @@
+#!/usr/bin/python3
+
+import sys
+from y2024.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', drivetrain.kDrivetrain)
+
+
+if __name__ == '__main__':
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2024/control_loops/superstructure/BUILD b/y2024/control_loops/superstructure/BUILD
new file mode 100644
index 0000000..996d28b
--- /dev/null
+++ b/y2024/control_loops/superstructure/BUILD
@@ -0,0 +1,151 @@
+load("//tools/build_rules:js.bzl", "ts_project")
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
+load("@com_github_google_flatbuffers//:typescript.bzl", "flatbuffer_ts_library")
+
+package(default_visibility = ["//visibility:public"])
+
+static_flatbuffer(
+ name = "superstructure_goal_fbs",
+ srcs = [
+ "superstructure_goal.fbs",
+ ],
+ deps = [
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops:profiled_subsystem_fbs",
+ ],
+)
+
+static_flatbuffer(
+ name = "superstructure_output_fbs",
+ srcs = [
+ "superstructure_output.fbs",
+ ],
+)
+
+static_flatbuffer(
+ name = "superstructure_status_fbs",
+ srcs = [
+ "superstructure_status.fbs",
+ ],
+ deps = [
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops:profiled_subsystem_fbs",
+ ],
+)
+
+flatbuffer_ts_library(
+ name = "superstructure_status_ts_fbs",
+ srcs = [
+ "superstructure_status.fbs",
+ ],
+ deps = [
+ "//frc971/control_loops:control_loops_ts_fbs",
+ "//frc971/control_loops:profiled_subsystem_ts_fbs",
+ ],
+)
+
+static_flatbuffer(
+ name = "superstructure_position_fbs",
+ srcs = [
+ "superstructure_position.fbs",
+ ],
+ deps = [
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops:profiled_subsystem_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_can_position_fbs",
+ ],
+)
+
+cc_library(
+ name = "superstructure_lib",
+ srcs = [
+ "superstructure.cc",
+ ],
+ hdrs = [
+ "superstructure.h",
+ ],
+ data = [
+ ],
+ deps = [
+ ":superstructure_goal_fbs",
+ ":superstructure_output_fbs",
+ ":superstructure_position_fbs",
+ ":superstructure_status_fbs",
+ "//aos:flatbuffer_merge",
+ "//aos/events:event_loop",
+ "//frc971/constants:constants_sender_lib",
+ "//frc971/control_loops:control_loop",
+ "//frc971/control_loops/drivetrain:drivetrain_can_position_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_status_fbs",
+ "//frc971/shooter_interpolation:interpolation",
+ "//frc971/zeroing:absolute_encoder",
+ "//frc971/zeroing:pot_and_absolute_encoder",
+ "//y2024:constants",
+ "//y2024/constants:constants_fbs",
+ "//y2024/constants:simulated_constants_sender",
+ ],
+)
+
+cc_binary(
+ name = "superstructure",
+ srcs = [
+ "superstructure_main.cc",
+ ],
+ deps = [
+ ":superstructure_lib",
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ ],
+)
+
+cc_test(
+ name = "superstructure_lib_test",
+ srcs = [
+ "superstructure_lib_test.cc",
+ ],
+ data = [
+ "//y2024:aos_config",
+ ],
+ deps = [
+ ":superstructure_goal_fbs",
+ ":superstructure_lib",
+ ":superstructure_output_fbs",
+ ":superstructure_position_fbs",
+ ":superstructure_status_fbs",
+ "//aos:json_to_flatbuffer",
+ "//aos:math",
+ "//aos/events/logging:log_writer",
+ "//aos/testing:googletest",
+ "//aos/time",
+ "//frc971/control_loops:capped_test_plant",
+ "//frc971/control_loops:control_loop_test",
+ "//frc971/control_loops:position_sensor_sim",
+ "//frc971/control_loops:subsystem_simulator",
+ "//frc971/control_loops:team_number_test_environment",
+ "//frc971/control_loops/drivetrain:drivetrain_status_fbs",
+ ],
+)
+
+cc_binary(
+ name = "superstructure_replay",
+ srcs = ["superstructure_replay.cc"],
+ deps = [
+ ":superstructure_lib",
+ "//aos:configuration",
+ "//aos:init",
+ "//aos/events:simulated_event_loop",
+ "//aos/events/logging:log_reader",
+ "//aos/network:team_number",
+ ],
+)
+
+ts_project(
+ name = "superstructure_plotter",
+ srcs = ["superstructure_plotter.ts"],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ "//aos/network/www:aos_plotter",
+ "//aos/network/www:colors",
+ "//aos/network/www:proxy",
+ ],
+)
diff --git a/y2024/control_loops/superstructure/superstructure.cc b/y2024/control_loops/superstructure/superstructure.cc
new file mode 100644
index 0000000..abb5536
--- /dev/null
+++ b/y2024/control_loops/superstructure/superstructure.cc
@@ -0,0 +1,78 @@
+#include "y2024/control_loops/superstructure/superstructure.h"
+
+#include "aos/events/event_loop.h"
+#include "aos/flatbuffer_merge.h"
+#include "aos/network/team_number.h"
+#include "frc971/shooter_interpolation/interpolation.h"
+#include "frc971/zeroing/wrap.h"
+
+DEFINE_bool(ignore_distance, false,
+ "If true, ignore distance when shooting and obay joystick_reader");
+
+namespace y2024 {
+namespace control_loops {
+namespace superstructure {
+
+using ::aos::monotonic_clock;
+
+using frc971::control_loops::AbsoluteEncoderProfiledJointStatus;
+using frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus;
+using frc971::control_loops::RelativeEncoderProfiledJointStatus;
+
+Superstructure::Superstructure(::aos::EventLoop *event_loop,
+ std::shared_ptr<const constants::Values> values,
+ const ::std::string &name)
+ : frc971::controls::ControlLoop<Goal, Position, Status, Output>(event_loop,
+ name),
+ values_(values),
+ constants_fetcher_(event_loop),
+ drivetrain_status_fetcher_(
+ event_loop->MakeFetcher<frc971::control_loops::drivetrain::Status>(
+ "/drivetrain")),
+ joystick_state_fetcher_(
+ event_loop->MakeFetcher<aos::JoystickState>("/aos")) {
+ event_loop->SetRuntimeRealtimePriority(30);
+}
+
+void Superstructure::RunIteration(const Goal *unsafe_goal,
+ 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)timestamp;
+ (void)unsafe_goal;
+ (void)position;
+
+ if (WasReset()) {
+ AOS_LOG(ERROR, "WPILib reset, restarting\n");
+ }
+
+ OutputT output_struct;
+ if (joystick_state_fetcher_.Fetch() &&
+ joystick_state_fetcher_->has_alliance()) {
+ alliance_ = joystick_state_fetcher_->alliance();
+ }
+ drivetrain_status_fetcher_.Fetch();
+
+ if (output) {
+ output->CheckOk(output->Send(Output::Pack(*output->fbb(), &output_struct)));
+ }
+
+ Status::Builder status_builder = status->MakeBuilder<Status>();
+ status_builder.add_zeroed(true);
+ status_builder.add_estopped(false);
+
+ (void)status->Send(status_builder.Finish());
+}
+
+double Superstructure::robot_velocity() const {
+ return (drivetrain_status_fetcher_.get() != nullptr
+ ? drivetrain_status_fetcher_->robot_speed()
+ : 0.0);
+}
+
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2024
\ No newline at end of file
diff --git a/y2024/control_loops/superstructure/superstructure.h b/y2024/control_loops/superstructure/superstructure.h
new file mode 100644
index 0000000..1d7cdf5
--- /dev/null
+++ b/y2024/control_loops/superstructure/superstructure.h
@@ -0,0 +1,54 @@
+#ifndef Y2024_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
+#define Y2024_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
+
+#include "aos/events/event_loop.h"
+#include "aos/json_to_flatbuffer.h"
+#include "frc971/constants/constants_sender_lib.h"
+#include "frc971/control_loops/control_loop.h"
+#include "frc971/control_loops/drivetrain/drivetrain_can_position_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
+#include "frc971/zeroing/absolute_encoder.h"
+#include "frc971/zeroing/pot_and_absolute_encoder.h"
+#include "y2024/constants.h"
+#include "y2024/constants/constants_generated.h"
+#include "y2024/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2024/control_loops/superstructure/superstructure_output_generated.h"
+#include "y2024/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2024/control_loops/superstructure/superstructure_status_generated.h"
+
+namespace y2024 {
+namespace control_loops {
+namespace superstructure {
+
+class Superstructure
+ : public ::frc971::controls::ControlLoop<Goal, Position, Status, Output> {
+ public:
+ explicit Superstructure(::aos::EventLoop *event_loop,
+ std::shared_ptr<const constants::Values> values,
+ const ::std::string &name = "/superstructure");
+
+ double robot_velocity() const;
+
+ protected:
+ virtual void RunIteration(const Goal *unsafe_goal, const Position *position,
+ aos::Sender<Output>::Builder *output,
+ aos::Sender<Status>::Builder *status) override;
+
+ private:
+ std::shared_ptr<const constants::Values> values_;
+ frc971::constants::ConstantsFetcher<Constants> constants_fetcher_;
+
+ aos::Fetcher<frc971::control_loops::drivetrain::Status>
+ drivetrain_status_fetcher_;
+ aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
+
+ aos::Alliance alliance_ = aos::Alliance::kInvalid;
+
+ DISALLOW_COPY_AND_ASSIGN(Superstructure);
+};
+
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2024
+
+#endif // Y2024_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
diff --git a/y2024/control_loops/superstructure/superstructure_goal.fbs b/y2024/control_loops/superstructure/superstructure_goal.fbs
new file mode 100644
index 0000000..1c4b3dd
--- /dev/null
+++ b/y2024/control_loops/superstructure/superstructure_goal.fbs
@@ -0,0 +1,9 @@
+include "frc971/control_loops/profiled_subsystem.fbs";
+
+namespace y2024.control_loops.superstructure;
+
+table Goal {
+}
+
+
+root_type Goal;
diff --git a/y2024/control_loops/superstructure/superstructure_lib_test.cc b/y2024/control_loops/superstructure/superstructure_lib_test.cc
new file mode 100644
index 0000000..85e16b2
--- /dev/null
+++ b/y2024/control_loops/superstructure/superstructure_lib_test.cc
@@ -0,0 +1,287 @@
+#include <chrono>
+#include <memory>
+
+#include "gtest/gtest.h"
+
+#include "aos/events/logging/log_writer.h"
+#include "frc971/control_loops/capped_test_plant.h"
+#include "frc971/control_loops/control_loop_test.h"
+#include "frc971/control_loops/position_sensor_sim.h"
+#include "frc971/control_loops/subsystem_simulator.h"
+#include "frc971/control_loops/team_number_test_environment.h"
+#include "frc971/zeroing/absolute_encoder.h"
+#include "y2024/constants/simulated_constants_sender.h"
+#include "y2024/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "y2024/control_loops/superstructure/superstructure.h"
+
+DEFINE_string(output_folder, "",
+ "If set, logs all channels to the provided logfile.");
+
+namespace y2024 {
+namespace control_loops {
+namespace superstructure {
+namespace testing {
+
+namespace chrono = std::chrono;
+
+using ::aos::monotonic_clock;
+using ::frc971::CreateProfileParameters;
+using ::frc971::control_loops::CappedTestPlant;
+using ::frc971::control_loops::
+ CreateStaticZeroingSingleDOFProfiledSubsystemGoal;
+using ::frc971::control_loops::PositionSensorSimulator;
+using ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal;
+using DrivetrainStatus = ::frc971::control_loops::drivetrain::Status;
+
+class SuperstructureSimulation {
+ public:
+ SuperstructureSimulation(::aos::EventLoop *event_loop, chrono::nanoseconds dt)
+ : event_loop_(event_loop),
+ dt_(dt),
+ superstructure_position_sender_(
+ event_loop_->MakeSender<Position>("/superstructure")),
+ superstructure_status_fetcher_(
+ event_loop_->MakeFetcher<Status>("/superstructure")),
+ superstructure_output_fetcher_(
+ event_loop_->MakeFetcher<Output>("/superstructure")) {
+ (void)dt_;
+ phased_loop_handle_ = event_loop_->AddPhasedLoop(
+ [this](int) {
+ // Skip this the first time.
+ if (!first_) {
+ EXPECT_TRUE(superstructure_output_fetcher_.Fetch());
+ EXPECT_TRUE(superstructure_status_fetcher_.Fetch());
+ }
+ first_ = false;
+ SendPositionMessage();
+ },
+ dt);
+ }
+
+ // Sends a queue message with the position of the superstructure.
+ void SendPositionMessage() {
+ ::aos::Sender<Position>::Builder builder =
+ superstructure_position_sender_.MakeBuilder();
+
+ Position::Builder position_builder = builder.MakeBuilder<Position>();
+ CHECK_EQ(builder.Send(position_builder.Finish()),
+ aos::RawSender::Error::kOk);
+ }
+
+ private:
+ ::aos::EventLoop *event_loop_;
+ const chrono::nanoseconds dt_;
+ ::aos::PhasedLoopHandler *phased_loop_handle_ = nullptr;
+
+ ::aos::Sender<Position> superstructure_position_sender_;
+ ::aos::Fetcher<Status> superstructure_status_fetcher_;
+ ::aos::Fetcher<Output> superstructure_output_fetcher_;
+
+ bool first_ = true;
+};
+
+class SuperstructureTest : public ::frc971::testing::ControlLoopTest {
+ public:
+ SuperstructureTest()
+ : ::frc971::testing::ControlLoopTest(
+ aos::configuration::ReadConfig("y2024/aos_config.json"),
+ std::chrono::microseconds(5050)),
+ values_(std::make_shared<constants::Values>(constants::MakeValues())),
+ simulated_constants_dummy_(SendSimulationConstants(
+ event_loop_factory(), 7971, "y2024/constants/test_constants.json")),
+ roborio_(aos::configuration::GetNode(configuration(), "roborio")),
+ logger_pi_(aos::configuration::GetNode(configuration(), "logger")),
+ superstructure_event_loop(MakeEventLoop("Superstructure", roborio_)),
+ superstructure_(superstructure_event_loop.get(), (values_)),
+ test_event_loop_(MakeEventLoop("test", roborio_)),
+ superstructure_goal_fetcher_(
+ test_event_loop_->MakeFetcher<Goal>("/superstructure")),
+ superstructure_goal_sender_(
+ test_event_loop_->MakeSender<Goal>("/superstructure")),
+ superstructure_status_fetcher_(
+ test_event_loop_->MakeFetcher<Status>("/superstructure")),
+ superstructure_output_fetcher_(
+ test_event_loop_->MakeFetcher<Output>("/superstructure")),
+ superstructure_position_fetcher_(
+ test_event_loop_->MakeFetcher<Position>("/superstructure")),
+ superstructure_position_sender_(
+ test_event_loop_->MakeSender<Position>("/superstructure")),
+ drivetrain_status_sender_(
+ test_event_loop_->MakeSender<DrivetrainStatus>("/drivetrain")),
+ superstructure_plant_event_loop_(MakeEventLoop("plant", roborio_)),
+ superstructure_plant_(superstructure_plant_event_loop_.get(), dt()) {
+ (void)values_;
+ set_team_id(frc971::control_loops::testing::kTeamNumber);
+
+ SetEnabled(true);
+
+ if (!FLAGS_output_folder.empty()) {
+ unlink(FLAGS_output_folder.c_str());
+ logger_event_loop_ = MakeEventLoop("logger", roborio_);
+ logger_ = std::make_unique<aos::logger::Logger>(logger_event_loop_.get());
+ logger_->StartLoggingOnRun(FLAGS_output_folder);
+ }
+ }
+
+ void VerifyNearGoal() {
+ superstructure_goal_fetcher_.Fetch();
+ superstructure_status_fetcher_.Fetch();
+
+ ASSERT_TRUE(superstructure_goal_fetcher_.get() != nullptr) << ": No goal";
+ ASSERT_TRUE(superstructure_status_fetcher_.get() != nullptr)
+ << ": No status";
+ }
+
+ void CheckIfZeroed() {
+ superstructure_status_fetcher_.Fetch();
+ ASSERT_TRUE(superstructure_status_fetcher_.get()->zeroed());
+ }
+
+ void WaitUntilZeroed() {
+ int i = 0;
+ do {
+ i++;
+ RunFor(dt());
+ superstructure_status_fetcher_.Fetch();
+ // 2 Seconds
+
+ ASSERT_LE(i, 2.0 / ::aos::time::DurationInSeconds(dt()));
+
+ // Since there is a delay when sending running, make sure we have a
+ // status before checking it.
+ } while (superstructure_status_fetcher_.get() == nullptr ||
+ !superstructure_status_fetcher_.get()->zeroed());
+ }
+
+ void SendRobotVelocity(double robot_velocity) {
+ SendDrivetrainStatus(robot_velocity, {0.0, 0.0}, 0.0);
+ }
+
+ void SendDrivetrainStatus(double robot_velocity, Eigen::Vector2d pos,
+ double theta) {
+ // Send a robot velocity to test compensation
+ auto builder = drivetrain_status_sender_.MakeBuilder();
+ auto drivetrain_status_builder = builder.MakeBuilder<DrivetrainStatus>();
+ drivetrain_status_builder.add_robot_speed(robot_velocity);
+ drivetrain_status_builder.add_estimated_left_velocity(robot_velocity);
+ drivetrain_status_builder.add_estimated_right_velocity(robot_velocity);
+ drivetrain_status_builder.add_x(pos.x());
+ drivetrain_status_builder.add_y(pos.y());
+ drivetrain_status_builder.add_theta(theta);
+ builder.CheckOk(builder.Send(drivetrain_status_builder.Finish()));
+ }
+
+ std::shared_ptr<const constants::Values> values_;
+ const bool simulated_constants_dummy_;
+
+ const aos::Node *const roborio_;
+ const aos::Node *const logger_pi_;
+
+ ::std::unique_ptr<::aos::EventLoop> superstructure_event_loop;
+ ::y2024::control_loops::superstructure::Superstructure superstructure_;
+ ::std::unique_ptr<::aos::EventLoop> test_event_loop_;
+ ::aos::PhasedLoopHandler *phased_loop_handle_ = nullptr;
+
+ ::aos::Fetcher<Goal> superstructure_goal_fetcher_;
+ ::aos::Sender<Goal> superstructure_goal_sender_;
+ ::aos::Fetcher<Status> superstructure_status_fetcher_;
+ ::aos::Fetcher<Output> superstructure_output_fetcher_;
+ ::aos::Fetcher<Position> superstructure_position_fetcher_;
+ ::aos::Sender<Position> superstructure_position_sender_;
+ ::aos::Sender<DrivetrainStatus> drivetrain_status_sender_;
+
+ ::std::unique_ptr<::aos::EventLoop> superstructure_plant_event_loop_;
+ SuperstructureSimulation superstructure_plant_;
+
+ std::unique_ptr<aos::EventLoop> logger_event_loop_;
+ std::unique_ptr<aos::logger::Logger> logger_;
+
+ const ::std::vector<::Eigen::Matrix<double, 3, 1>> points_;
+};
+
+// Tests that the superstructure 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>();
+
+ 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);
+ WaitUntilZeroed();
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ }
+
+ // Give it a lot of time to get there.
+ RunFor(chrono::seconds(15));
+
+ VerifyNearGoal();
+}
+
+// Makes sure that the voltage on a motor is properly pulled back after
+// saturation such that we don't get weird or bad (e.g. oscillating)
+// behaviour.
+TEST_F(SuperstructureTest, SaturationTest) {
+ SetEnabled(true);
+
+ // Zero it before we move.
+ WaitUntilZeroed();
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ }
+ RunFor(chrono::seconds(20));
+ VerifyNearGoal();
+
+ // Try a low acceleration move with a high max velocity and verify the
+ // acceleration is capped like expected.
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ }
+
+ // TODO(Milo): Make this a sane time
+ RunFor(chrono::seconds(20));
+ VerifyNearGoal();
+}
+
+// Tests that the loop zeroes when run for a while without a goal.
+TEST_F(SuperstructureTest, ZeroNoGoal) {
+ SetEnabled(true);
+ WaitUntilZeroed();
+ RunFor(chrono::seconds(2));
+}
+
+// Tests that running disabled works
+TEST_F(SuperstructureTest, DisableTest) {
+ RunFor(chrono::seconds(2));
+ CheckIfZeroed();
+}
+} // namespace testing
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2024
diff --git a/y2024/control_loops/superstructure/superstructure_main.cc b/y2024/control_loops/superstructure/superstructure_main.cc
new file mode 100644
index 0000000..58932c7
--- /dev/null
+++ b/y2024/control_loops/superstructure/superstructure_main.cc
@@ -0,0 +1,28 @@
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "y2024/control_loops/superstructure/superstructure.h"
+
+DEFINE_string(arm_trajectories, "arm_trajectories_generated.bfbs",
+ "The path to the generated arm trajectories bfbs file.");
+
+using y2024::control_loops::superstructure::Superstructure;
+
+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());
+
+ frc971::constants::WaitForConstants<y2024::Constants>(&config.message());
+
+ std::shared_ptr<const y2024::constants::Values> values =
+ std::make_shared<const y2024::constants::Values>(
+ y2024::constants::MakeValues());
+ Superstructure superstructure(&event_loop, values);
+
+ event_loop.Run();
+
+ return 0;
+}
diff --git a/y2024/control_loops/superstructure/superstructure_output.fbs b/y2024/control_loops/superstructure/superstructure_output.fbs
new file mode 100644
index 0000000..6f0df4a
--- /dev/null
+++ b/y2024/control_loops/superstructure/superstructure_output.fbs
@@ -0,0 +1,6 @@
+namespace y2024.control_loops.superstructure;
+
+table Output {
+}
+
+root_type Output;
diff --git a/y2024/control_loops/superstructure/superstructure_plotter.ts b/y2024/control_loops/superstructure/superstructure_plotter.ts
new file mode 100644
index 0000000..3498f77
--- /dev/null
+++ b/y2024/control_loops/superstructure/superstructure_plotter.ts
@@ -0,0 +1,30 @@
+// Provides a plot for debugging robot state-related issues.
+import {AosPlotter} from '../../../aos/network/www/aos_plotter';
+import {BLUE, BROWN, CYAN, GREEN, PINK, RED, WHITE} from '../../../aos/network/www/colors';
+import * as proxy from '../../../aos/network/www/proxy';
+
+import Connection = proxy.Connection;
+
+const TIME = AosPlotter.TIME;
+const DEFAULT_WIDTH = AosPlotter.DEFAULT_WIDTH * 2;
+const DEFAULT_HEIGHT = AosPlotter.DEFAULT_HEIGHT * 3;
+
+export function plotSuperstructure(conn: Connection, element: Element): void {
+ const aosPlotter = new AosPlotter(conn);
+ //const goal = aosPlotter.addMessageSource(
+ // '/superstructure', 'y2024.control_loops.superstructure.Goal');
+ //const output = aosPlotter.addMessageSource(
+ // '/superstructure', 'y2024.control_loops.superstructure.Output');
+ //const status = aosPlotter.addMessageSource(
+ // '/superstructure', 'y2024.control_loops.superstructure.Status');
+ const position = aosPlotter.addMessageSource(
+ '/superstructure', 'y2024.control_loops.superstructure.Position');
+ //const robotState = aosPlotter.addMessageSource('/aos', 'aos.RobotState');
+
+ const positionPlot =
+ aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
+ positionPlot.plot.getAxisLabels().setTitle('States');
+ positionPlot.plot.getAxisLabels().setXLabel(TIME);
+ positionPlot.plot.getAxisLabels().setYLabel('wonky state units');
+ positionPlot.plot.setDefaultYRange([-1.0, 2.0]);
+}
diff --git a/y2024/control_loops/superstructure/superstructure_position.fbs b/y2024/control_loops/superstructure/superstructure_position.fbs
new file mode 100644
index 0000000..83d3d23
--- /dev/null
+++ b/y2024/control_loops/superstructure/superstructure_position.fbs
@@ -0,0 +1,8 @@
+include "frc971/control_loops/control_loops.fbs";
+
+namespace y2024.control_loops.superstructure;
+
+table Position {
+}
+
+root_type Position;
diff --git a/y2024/control_loops/superstructure/superstructure_replay.cc b/y2024/control_loops/superstructure/superstructure_replay.cc
new file mode 100644
index 0000000..bd7f9fd
--- /dev/null
+++ b/y2024/control_loops/superstructure/superstructure_replay.cc
@@ -0,0 +1,75 @@
+// This binary allows us to replay the superstructure code over existing
+// logfile. When you run this code, it generates a new logfile with the data all
+// replayed, so that it can then be run through the plotting tool or analyzed
+// in some other way. The original superstructure status data will be on the
+// /original/superstructure channel.
+#include "gflags/gflags.h"
+
+#include "aos/events/logging/log_reader.h"
+#include "aos/events/logging/log_writer.h"
+#include "aos/events/simulated_event_loop.h"
+#include "aos/init.h"
+#include "aos/json_to_flatbuffer.h"
+#include "aos/logging/log_message_generated.h"
+#include "aos/network/team_number.h"
+#include "y2024/constants.h"
+#include "y2024/control_loops/superstructure/superstructure.h"
+
+DEFINE_int32(team, 971, "Team number to use for logfile replay.");
+DEFINE_string(output_folder, "/tmp/superstructure_replay/",
+ "Logs all channels to the provided logfile.");
+
+int main(int argc, char **argv) {
+ aos::InitGoogle(&argc, &argv);
+
+ aos::network::OverrideTeamNumber(FLAGS_team);
+
+ // open logfiles
+ aos::logger::LogReader reader(
+ aos::logger::SortParts(aos::logger::FindLogs(argc, argv)));
+ // TODO(james): Actually enforce not sending on the same buses as the logfile
+ // spews out.
+ reader.RemapLoggedChannel("/superstructure",
+ "y2024.control_loops.superstructure.Status");
+ reader.RemapLoggedChannel("/superstructure",
+ "y2024.control_loops.superstructure.Output");
+
+ aos::SimulatedEventLoopFactory factory(reader.configuration());
+ reader.Register(&factory);
+
+ aos::NodeEventLoopFactory *roborio =
+ factory.GetNodeEventLoopFactory("roborio");
+
+ unlink(FLAGS_output_folder.c_str());
+ std::unique_ptr<aos::EventLoop> logger_event_loop =
+ roborio->MakeEventLoop("logger");
+ auto logger = std::make_unique<aos::logger::Logger>(logger_event_loop.get());
+ logger->StartLoggingOnRun(FLAGS_output_folder);
+
+ roborio->OnStartup([roborio]() {
+ roborio->AlwaysStart<y2024::control_loops::superstructure::Superstructure>(
+ "superstructure", std::make_shared<y2024::constants::Values>(
+ y2024::constants::MakeValues()));
+ });
+
+ std::unique_ptr<aos::EventLoop> print_loop = roborio->MakeEventLoop("print");
+ print_loop->SkipAosLog();
+ print_loop->MakeWatcher(
+ "/aos", [&print_loop](const aos::logging::LogMessageFbs &msg) {
+ LOG(INFO) << print_loop->context().monotonic_event_time << " "
+ << aos::FlatbufferToJson(&msg);
+ });
+ print_loop->MakeWatcher(
+ "/superstructure",
+ [&](const y2024::control_loops::superstructure::Status &status) {
+ if (status.estopped()) {
+ LOG(ERROR) << "Estopped";
+ }
+ });
+
+ factory.Run();
+
+ reader.Deregister();
+
+ return 0;
+}
diff --git a/y2024/control_loops/superstructure/superstructure_status.fbs b/y2024/control_loops/superstructure/superstructure_status.fbs
new file mode 100644
index 0000000..7e8cc27
--- /dev/null
+++ b/y2024/control_loops/superstructure/superstructure_status.fbs
@@ -0,0 +1,14 @@
+include "frc971/control_loops/control_loops.fbs";
+include "frc971/control_loops/profiled_subsystem.fbs";
+
+namespace y2024.control_loops.superstructure;
+
+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);
+}
+
+root_type Status;
diff --git a/y2024/copy_logs.sh b/y2024/copy_logs.sh
new file mode 100755
index 0000000..4d5669c
--- /dev/null
+++ b/y2024/copy_logs.sh
@@ -0,0 +1,29 @@
+#!/bin/bash
+
+# Helper script to copy most recent logs off of the pis
+
+set -e
+
+ROBOT_PREFIX="79" # ..71 (Should be one of 79, 89, 99, or 9)
+PI_LIST="2 3" # Should be some set of {1,2,3,4,5,6}
+
+LOG_FILE_PATH=/media/sda1/fbs_log-current
+if [[ -z $1 || ! -d $1 ]]; then
+ echo "Please specify the base directory to store the logs ('$1' not found)"
+ exit -1
+fi
+
+# Create output directory based on given directory + a timestamp
+OUTPUT_DIR=$1"/"`date +"%Y-%m-%dT%H-%M-%S"`
+mkdir ${OUTPUT_DIR}
+
+echo "Copying logs from the robot ${ROBOT_PREFIX}71 and pis ${PI_LIST}"
+echo "Storing logs in folder ${OUTPUT_DIR}"
+
+for pi in $PI_LIST; do
+ echo "========================================================"
+ echo "Copying logs from pi-${ROBOT_PREFIX}71-$pi"
+ echo "========================================================"
+ scp -r pi@10.${ROBOT_PREFIX}.71.10${pi}:${LOG_FILE_PATH} ${OUTPUT_DIR}/fbs_log-pi${pi}
+done
+
diff --git a/y2024/joystick_reader.cc b/y2024/joystick_reader.cc
new file mode 100644
index 0000000..ebc2485
--- /dev/null
+++ b/y2024/joystick_reader.cc
@@ -0,0 +1,87 @@
+#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/control_loops/drivetrain/drivetrain_base.h"
+#include "y2024/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2024/control_loops/superstructure/superstructure_status_generated.h"
+using frc971::CreateProfileParameters;
+using frc971::control_loops::CreateStaticZeroingSingleDOFProfiledSubsystemGoal;
+using frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal;
+using frc971::input::driver_station::ButtonLocation;
+using frc971::input::driver_station::ControlBit;
+using frc971::input::driver_station::JoystickAxis;
+using frc971::input::driver_station::POVLocation;
+using Side = frc971::control_loops::drivetrain::RobotSide;
+
+namespace y2024 {
+namespace input {
+namespace joysticks {
+
+class Reader : public ::frc971::input::ActionJoystickInput {
+ public:
+ Reader(::aos::EventLoop *event_loop)
+ : ::frc971::input::ActionJoystickInput(
+ event_loop,
+ ::y2024::control_loops::drivetrain::GetDrivetrainConfig(),
+ ::frc971::input::DrivetrainInputReader::InputType::kPistol,
+ {.use_redundant_joysticks = true}),
+ superstructure_goal_sender_(
+ event_loop->MakeSender<control_loops::superstructure::Goal>(
+ "/superstructure")),
+ superstructure_status_fetcher_(
+ event_loop->MakeFetcher<control_loops::superstructure::Status>(
+ "/superstructure")) {}
+
+ void AutoEnded() override { AOS_LOG(INFO, "Auto ended.\n"); }
+
+ void HandleTeleop(
+ const ::frc971::input::driver_station::Data &data) override {
+ (void)data;
+ superstructure_status_fetcher_.Fetch();
+ if (!superstructure_status_fetcher_.get()) {
+ AOS_LOG(ERROR, "Got no superstructure status message.\n");
+ return;
+ }
+ }
+
+ private:
+ ::aos::Sender<control_loops::superstructure::Goal>
+ superstructure_goal_sender_;
+ ::aos::Fetcher<control_loops::superstructure::Status>
+ superstructure_status_fetcher_;
+};
+
+} // namespace joysticks
+} // namespace input
+} // namespace y2024
+
+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::input::joysticks::Reader reader(&event_loop);
+
+ event_loop.Run();
+
+ return 0;
+}
diff --git a/y2024/joystick_republish.cc b/y2024/joystick_republish.cc
new file mode 100644
index 0000000..ae6f340
--- /dev/null
+++ b/y2024/joystick_republish.cc
@@ -0,0 +1,34 @@
+#include <sys/resource.h>
+#include <sys/time.h>
+
+#include "glog/logging.h"
+
+#include "aos/configuration.h"
+#include "aos/events/shm_event_loop.h"
+#include "aos/flatbuffer_merge.h"
+#include "aos/init.h"
+#include "frc971/input/joystick_state_generated.h"
+
+DEFINE_string(config, "aos_config.json", "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());
+
+ aos::Sender<aos::JoystickState> sender(
+ event_loop.MakeSender<aos::JoystickState>("/imu/aos"));
+
+ event_loop.MakeWatcher(
+ "/roborio/aos", [&](const aos::JoystickState &joystick_state) {
+ auto builder = sender.MakeBuilder();
+ flatbuffers::Offset<aos::JoystickState> state_fbs =
+ aos::CopyFlatBuffer(&joystick_state, builder.fbb());
+ builder.CheckOk(builder.Send(state_fbs));
+ });
+
+ event_loop.Run();
+ return 0;
+}
diff --git a/y2024/log_web_proxy.sh b/y2024/log_web_proxy.sh
new file mode 100755
index 0000000..8161e7f
--- /dev/null
+++ b/y2024/log_web_proxy.sh
@@ -0,0 +1 @@
+./aos/network/log_web_proxy_main --data_dir=y2024/www $@
diff --git a/y2024/pi_send_joystick.sh b/y2024/pi_send_joystick.sh
new file mode 100755
index 0000000..9b60e02
--- /dev/null
+++ b/y2024/pi_send_joystick.sh
@@ -0,0 +1,18 @@
+#!/usr/bin/sh
+
+# Helper script to spoof joystick state of robot enabled, to triggger image logging
+
+# Currently, this is going through pi6. Need to set the right IP address for the bot
+imu_pi6="pi@10.79.71.106"
+
+# TODO(milind): add logger in the future
+echo "Sending Joystick command '$1' to $imu_pi6"
+ssh ${imu_pi6} "bin/aos_send /imu/aos aos.JoystickState '{\"enabled\": $1}'"
+
+if [ $1 = "false" ]
+then
+ # This extra sleep is necessary to make sure the logs rotate to a new file
+ sleep 6
+ echo "Sending Joystick command '$1' to $imu_pi6"
+ ssh ${imu_pi6} "bin/aos_send /imu/aos aos.JoystickState '{\"enabled\": $1}'"
+fi
diff --git a/y2024/vision/BUILD b/y2024/vision/BUILD
new file mode 100644
index 0000000..98b5197
--- /dev/null
+++ b/y2024/vision/BUILD
@@ -0,0 +1,24 @@
+filegroup(
+ name = "image_streamer_start",
+ srcs = ["image_streamer_start.sh"],
+ visibility = ["//visibility:public"],
+)
+
+cc_binary(
+ name = "image_logger",
+ srcs = [
+ "image_logger.cc",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos:configuration",
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ "//aos/events/logging:log_writer",
+ "//aos/logging:log_namer",
+ "//frc971/input:joystick_state_fbs",
+ "@com_github_gflags_gflags//:gflags",
+ "@com_github_google_glog//:glog",
+ ],
+)
diff --git a/y2024/vision/README.md b/y2024/vision/README.md
new file mode 100644
index 0000000..c81581f
--- /dev/null
+++ b/y2024/vision/README.md
@@ -0,0 +1,25 @@
+How to use the extrinsic calibration for camera-to-camera calibration
+
+This all assumes you have cameras that have been intrinsically
+calibrated, and that pi1 has a valid extrinsic calibration (from robot
+origin / IMU to the pi1 camera).
+
+It by default will compute the extrinsics for each of the other cameras (pi2,
+pi3, pi4) relative to the robot origin (IMU origin).
+
+Steps:
+* Place two Aruco Diamond markers about 1 meter apart
+ (center-to-center) at a height so that they will be in view of the
+ cameras when the robot is about 1-2m away
+* Start with the robot in a position that both markers are fully in
+ view by one camera
+* Enable logging for all cameras
+* Rotate the robot in place through a full circle, so that each camera sees both tags, and at times just one tag.
+* Stop the logging and copy the files to your laptop
+* Run the calibration code on the resulting files, e.g.,
+ * `bazel run -c opt //y2023/vision:calibrate_multi_cameras -- /data/PATH_TO_LOGS --team_number 971 --target_type charuco_diamond
+ * Can add "--visualize" flag to see the camera views and marker detections
+ * I've sometimes found it necessary to add the "--skip_missing_forwarding_entries" flag-- as guided by the logging messages
+* From the output, copy the calculated ("Updated") extrinsic into the
+ corresponding calibration file for the right robot and camera in the
+ calib_files directory
diff --git a/y2024/vision/foxglove_image_converter.cc b/y2024/vision/foxglove_image_converter.cc
new file mode 100644
index 0000000..bfaafc8
--- /dev/null
+++ b/y2024/vision/foxglove_image_converter.cc
@@ -0,0 +1,20 @@
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "frc971/vision/foxglove_image_converter_lib.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::vision::FoxgloveImageConverter converter(
+ &event_loop, "/camera", "/camera",
+ frc971::vision::ImageCompression::kJpeg);
+
+ event_loop.Run();
+}
diff --git a/y2024/vision/image_logger.cc b/y2024/vision/image_logger.cc
new file mode 100644
index 0000000..45e25f6
--- /dev/null
+++ b/y2024/vision/image_logger.cc
@@ -0,0 +1,112 @@
+#include <sys/resource.h>
+#include <sys/time.h>
+
+#include "gflags/gflags.h"
+#include "glog/logging.h"
+
+#include "aos/configuration.h"
+#include "aos/events/logging/log_writer.h"
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "aos/logging/log_namer.h"
+#include "frc971/input/joystick_state_generated.h"
+
+DEFINE_string(config, "aos_config.json", "Config file to use.");
+
+DEFINE_double(rotate_every, 0.0,
+ "If set, rotate the logger after this many seconds");
+DECLARE_int32(flush_size);
+DEFINE_double(disabled_time, 5.0,
+ "Continue logging if disabled for this amount of time or less");
+DEFINE_bool(direct, false,
+ "If true, write using O_DIRECT and write 512 byte aligned blocks "
+ "whenever possible.");
+
+std::unique_ptr<aos::logger::MultiNodeFilesLogNamer> MakeLogNamer(
+ aos::EventLoop *event_loop) {
+ std::optional<std::string> log_name =
+ aos::logging::MaybeGetLogName("fbs_log");
+
+ if (!log_name.has_value()) {
+ return nullptr;
+ }
+
+ return std::make_unique<aos::logger::MultiNodeFilesLogNamer>(
+ event_loop, std::make_unique<aos::logger::RenamableFileBackend>(
+ absl::StrCat(log_name.value(), "/"), FLAGS_direct));
+}
+
+int main(int argc, char *argv[]) {
+ gflags::SetUsageMessage(
+ "This program provides a simple logger binary that logs all SHMEM data "
+ "directly to a file specified at the command line when the robot is "
+ "enabled and for a bit of time after.");
+ aos::InitGoogle(&argc, &argv);
+
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig(FLAGS_config);
+
+ aos::ShmEventLoop event_loop(&config.message());
+
+ bool logging = false;
+ bool enabled = false;
+ aos::monotonic_clock::time_point last_disable_time =
+ event_loop.monotonic_now();
+ aos::monotonic_clock::time_point last_rotation_time =
+ event_loop.monotonic_now();
+ aos::logger::Logger logger(&event_loop);
+
+ if (FLAGS_rotate_every != 0.0) {
+ logger.set_on_logged_period([&](aos::monotonic_clock::time_point) {
+ const auto now = event_loop.monotonic_now();
+ if (logging && now > last_rotation_time + std::chrono::duration<double>(
+ FLAGS_rotate_every)) {
+ logger.Rotate();
+ last_rotation_time = now;
+ }
+ });
+ }
+
+ event_loop.OnRun([]() {
+ errno = 0;
+ setpriority(PRIO_PROCESS, 0, -20);
+ PCHECK(errno == 0) << ": Renicing to -20 failed.";
+ });
+
+ event_loop.MakeWatcher(
+ "/imu/aos", [&](const aos::JoystickState &joystick_state) {
+ const auto timestamp = event_loop.context().monotonic_event_time;
+ // Store the last time we got disabled
+ if (enabled && !joystick_state.enabled()) {
+ last_disable_time = timestamp;
+ }
+ enabled = joystick_state.enabled();
+
+ if (!logging && enabled) {
+ auto log_namer = MakeLogNamer(&event_loop);
+ if (log_namer == nullptr) {
+ return;
+ }
+
+ // Start logging if we just got enabled
+ LOG(INFO) << "Starting logging";
+ logger.StartLogging(std::move(log_namer));
+ logging = true;
+ last_rotation_time = event_loop.monotonic_now();
+ } else if (logging && !enabled &&
+ (timestamp - last_disable_time) >
+ std::chrono::duration<double>(FLAGS_disabled_time)) {
+ // Stop logging if we've been disabled for a non-negligible amount of
+ // time
+ LOG(INFO) << "Stopping logging";
+ logger.StopLogging(event_loop.monotonic_now());
+ logging = false;
+ }
+ });
+
+ event_loop.Run();
+
+ LOG(INFO) << "Shutting down";
+
+ return 0;
+}
diff --git a/y2024/vision/image_streamer_start.sh b/y2024/vision/image_streamer_start.sh
new file mode 100755
index 0000000..48d9da7
--- /dev/null
+++ b/y2024/vision/image_streamer_start.sh
@@ -0,0 +1,22 @@
+#!/bin/bash
+
+# Some configurations to avoid dropping frames
+# 640x480@30fps, 400x300@60fps.
+# Bitrate 500000-1500000
+WIDTH=640
+HEIGHT=480
+BITRATE=1500000
+LISTEN_ON="/camera/downsized"
+# Don't interfere with field webpage
+STREAMING_PORT=1181
+
+# Handle weirdness with openssl and gstreamer
+export OPENSSL_CONF=""
+
+# Enable for verbose logging
+#export GST_DEBUG=4
+
+export LD_LIBRARY_PATH=/usr/lib/aarch64-linux-gnu/gstreamer-1.0
+
+exec ./image_streamer --width=$WIDTH --height=$HEIGHT --bitrate=$BITRATE --listen_on=$LISTEN_ON --config=aos_config.json --streaming_port=$STREAMING_PORT
+
diff --git a/y2024/vision/localization_verifier.cc b/y2024/vision/localization_verifier.cc
new file mode 100644
index 0000000..c16f874
--- /dev/null
+++ b/y2024/vision/localization_verifier.cc
@@ -0,0 +1,108 @@
+#include "glog/logging.h"
+
+#include "aos/init.h"
+#include "frc971/constants/constants_sender_lib.h"
+#include "frc971/control_loops/drivetrain/localization/localizer_output_generated.h"
+#include "frc971/vision/vision_generated.h"
+#include "y2023/localizer/localizer.h"
+#include "y2023/localizer/utils.h"
+
+DEFINE_string(config, "aos_config.json", "Path to the config file to use.");
+
+DEFINE_uint64(min_april_id, 1,
+ "Minimum april id to consider as part of the field and ignore");
+DEFINE_uint64(max_april_id, 8,
+ "Maximum april id to consider as part of the field and ignore");
+
+// This binary allows us to place extra april tags on the field and verify
+// that we can compute their field pose correctly
+
+namespace y2023::vision {
+
+using localizer::Localizer;
+
+Localizer::Transform LocalizerOutputToTransform(
+ const frc971::controls::LocalizerOutput &localizer) {
+ const auto T_field_robot =
+ Eigen::Translation3d(localizer.x(), localizer.y(), 0.0);
+
+ Eigen::AngleAxisd robot_yaw_angle(localizer.theta(),
+ Eigen::Vector3d::UnitZ());
+ const auto R_field_robot = Eigen::Quaterniond(robot_yaw_angle);
+ const Localizer::Transform H_field_robot =
+ (T_field_robot * R_field_robot).matrix();
+ return H_field_robot;
+}
+
+void HandleDetections(
+ const frc971::vision::TargetMap &detections,
+ const Localizer::Transform &H_robot_camera,
+ aos::Fetcher<frc971::controls::LocalizerOutput> *localizer_fetcher) {
+ localizer_fetcher->Fetch();
+ CHECK(localizer_fetcher->get());
+
+ for (const auto *target_pose : *detections.target_poses()) {
+ // Only look at april tags not part of the field
+ if (target_pose->id() >= FLAGS_min_april_id &&
+ target_pose->id() <= FLAGS_max_april_id) {
+ continue;
+ }
+
+ const Localizer::Transform H_camera_target =
+ localizer::PoseToTransform(target_pose);
+ const Localizer::Transform H_field_robot =
+ LocalizerOutputToTransform(*localizer_fetcher->get());
+
+ // Get the field-based target pose using the detection, extrinsics, and
+ // localizer output
+ const Localizer::Transform H_field_target =
+ H_field_robot * H_robot_camera * H_camera_target;
+
+ LOG(INFO) << "Field to target " << target_pose->id();
+ LOG(INFO) << "H_field_robot " << H_field_robot;
+ LOG(INFO) << "H_robot_camera " << H_robot_camera;
+ LOG(INFO) << "H_camera_target " << H_camera_target;
+ LOG(INFO) << "Transform: " << H_field_target;
+ LOG(INFO) << "Translation: "
+ << Eigen::Affine3d(H_field_target).translation();
+ LOG(INFO);
+ }
+}
+
+void LocalizationVerifierMain() {
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig(FLAGS_config);
+
+ frc971::constants::WaitForConstants<Constants>(&config.message());
+
+ aos::ShmEventLoop event_loop(&config.message());
+
+ frc971::constants::ConstantsFetcher<Constants> constants_fetcher(&event_loop);
+
+ aos::Fetcher<frc971::controls::LocalizerOutput> localizer_fetcher =
+ event_loop.MakeFetcher<frc971::controls::LocalizerOutput>("/localizer");
+
+ for (const auto *camera : *constants_fetcher.constants().cameras()) {
+ CHECK(camera->has_calibration());
+ Localizer::Transform H_robot_camera =
+ frc971::control_loops::drivetrain::FlatbufferToTransformationMatrix(
+ *camera->calibration()->fixed_extrinsics());
+
+ const std::string_view pi_name =
+ camera->calibration()->node_name()->string_view();
+ event_loop.MakeWatcher(
+ absl::StrCat("/", pi_name, "/camera"),
+ [H_robot_camera,
+ &localizer_fetcher](const frc971::vision::TargetMap &target_map) {
+ HandleDetections(target_map, H_robot_camera, &localizer_fetcher);
+ });
+ }
+ event_loop.Run();
+}
+
+} // namespace y2023::vision
+
+int main(int argc, char **argv) {
+ aos::InitGoogle(&argc, &argv);
+ y2023::vision::LocalizationVerifierMain();
+}
diff --git a/y2024/vision/maps/BUILD b/y2024/vision/maps/BUILD
new file mode 100644
index 0000000..38191a4
--- /dev/null
+++ b/y2024/vision/maps/BUILD
@@ -0,0 +1,7 @@
+filegroup(
+ name = "maps",
+ srcs = glob([
+ "*.json",
+ ]),
+ visibility = ["//visibility:public"],
+)
diff --git a/y2024/wpilib_interface.cc b/y2024/wpilib_interface.cc
new file mode 100644
index 0000000..8c1cfd3
--- /dev/null
+++ b/y2024/wpilib_interface.cc
@@ -0,0 +1,292 @@
+#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/dma.h"
+#include "frc971/wpilib/drivetrain_writer.h"
+#include "frc971/wpilib/encoder_and_potentiometer.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/constants.h"
+#include "y2024/control_loops/superstructure/superstructure_output_generated.h"
+#include "y2024/control_loops/superstructure/superstructure_position_generated.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::constants::Values;
+namespace superstructure = ::y2024::control_loops::superstructure;
+namespace drivetrain = ::y2024::control_loops::drivetrain;
+namespace chrono = ::std::chrono;
+using std::make_unique;
+
+namespace y2024 {
+namespace wpilib {
+namespace {
+
+constexpr double kMaxBringupPower = 12.0;
+
+// TODO(Brian): Fix the interpretation of the result of GetRaw here and in the
+// DMA stuff and then removing the * 2.0 in *_translate.
+// The low bit is direction.
+
+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
+
+static constexpr int kCANFalconCount = 6;
+static constexpr units::frequency::hertz_t kCANUpdateFreqHz = 200_Hz;
+
+// 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")),
+ superstructure_position_sender_(
+ event_loop->MakeSender<superstructure::Position>(
+ "/superstructure")),
+ drivetrain_position_sender_(
+ event_loop
+ ->MakeSender<::frc971::control_loops::drivetrain::Position>(
+ "/drivetrain")),
+ gyro_sender_(event_loop->MakeSender<::frc971::sensors::GyroReading>(
+ "/drivetrain")){};
+ 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 {
+ {
+ auto builder = superstructure_position_sender_.MakeBuilder();
+
+ superstructure::Position::Builder position_builder =
+ builder.MakeBuilder<superstructure::Position>();
+ builder.CheckOk(builder.Send(position_builder.Finish()));
+ }
+
+ {
+ 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()));
+ }
+ }
+
+ private:
+ std::shared_ptr<const Values> values_;
+
+ aos::Sender<frc971::autonomous::AutonomousMode> auto_mode_sender_;
+ aos::Sender<superstructure::Position> superstructure_position_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);
+ 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_yaw_rate_input(make_unique<frc::DigitalInput>(0));
+
+ AddLoop(&sensor_reader_event_loop);
+
+ // Thread 4.
+ // Set up CAN.
+ if (!FLAGS_ctre_diag_server) {
+ c_Phoenix_Diagnostics_SetSecondsToStart(-1);
+ c_Phoenix_Diagnostics_Dispose();
+ }
+
+ 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_output_event_loop(&config.message());
+ can_output_event_loop.set_name("CANOutputWriter");
+
+ // Thread 5
+ // Set up superstructure output.
+ ::aos::ShmEventLoop output_event_loop(&config.message());
+ output_event_loop.set_name("PWMOutputWriter");
+
+ AddLoop(&output_event_loop);
+
+ // Thread 6
+
+ RunLoops();
+ }
+};
+
+} // namespace wpilib
+} // namespace y2024
+
+AOS_ROBOT_CLASS(::y2024::wpilib::WPILibRobot);
diff --git a/y2024/www/BUILD b/y2024/www/BUILD
new file mode 100644
index 0000000..726a354
--- /dev/null
+++ b/y2024/www/BUILD
@@ -0,0 +1,67 @@
+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",
+ ]) + ["2024.png"],
+ visibility = ["//visibility:public"],
+)
+
+#Need to add 2024 field png
+genrule(
+ name = "2024_field_png",
+ srcs = ["//third_party/y2023/field:pictures"],
+ outs = ["2024.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_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",
+ "//y2024/control_loops/superstructure:superstructure_status_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:__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/www/constants.ts b/y2024/www/constants.ts
new file mode 100644
index 0000000..97d6e28
--- /dev/null
+++ b/y2024/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 + 11.25 * IN_TO_M;
+export const FIELD_LENGTH = 54 * FT_TO_M + 3.25 * IN_TO_M;
+
diff --git a/y2024/www/field.html b/y2024/www/field.html
new file mode 100644
index 0000000..8c3b291
--- /dev/null
+++ b/y2024/www/field.html
@@ -0,0 +1,87 @@
+<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>
+
+ <table>
+ <tr>
+ <th colspan="2">Images</th>
+ </tr>
+ <tr>
+ <td> Images Accepted </td>
+ <td id="images_accepted"> NA </td>
+ </tr>
+ </table>
+
+ <table>
+ <tr>
+ <th colspan="2">Superstructure</th>
+ </tr>
+ <!-- TODO: Add superstructure state -->
+ </table>
+ <table>
+ <tr>
+ <th colspan="2">Game Piece</th>
+ </tr>
+ <tr>
+ <td>Game Piece Held</td>
+ <td id="game_piece"> NA </td>
+ </tr>
+ <tr>
+ <td>Game Piece Position (+ = left, 0 = empty)</td>
+ <td id="game_piece_position"> NA </td>
+ </tr>
+ </table>
+
+ <h3>Zeroing Faults:</h3>
+ <p id="zeroing_faults"> NA </p>
+ </div>
+ <div id="middle_readouts">
+ <div id="vision_readouts">
+ </div>
+ <div id="message_bridge_status">
+ <div>
+ <div>Node</div>
+ <div>Client</div>
+ <div>Server</div>
+ </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>
+ </table>
+ </body>
+</html>
+
diff --git a/y2024/www/field_handler.ts b/y2024/www/field_handler.ts
new file mode 100644
index 0000000..6c5f2e3
--- /dev/null
+++ b/y2024/www/field_handler.ts
@@ -0,0 +1,25 @@
+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 {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'
+
+
+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;
+
+export class FieldHandler {
+ constructor(private readonly connection: Connection) {
+ }
+}
diff --git a/y2024/www/field_main.ts b/y2024/www/field_main.ts
new file mode 100644
index 0000000..24bc23a
--- /dev/null
+++ b/y2024/www/field_main.ts
@@ -0,0 +1,11 @@
+import {Connection} from '../../aos/network/www/proxy';
+
+import {FieldHandler} from './field_handler';
+
+const conn = new Connection();
+
+conn.connect();
+
+const fieldHandler = new FieldHandler(conn);
+
+
diff --git a/y2024/www/index.html b/y2024/www/index.html
new file mode 100644
index 0000000..e4e185e
--- /dev/null
+++ b/y2024/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/www/plotter.html b/y2024/www/plotter.html
new file mode 100644
index 0000000..629ceaa
--- /dev/null
+++ b/y2024/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/www/styles.css b/y2024/www/styles.css
new file mode 100644
index 0000000..c2c44d2
--- /dev/null
+++ b/y2024/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/y2024.json b/y2024/y2024.json
new file mode 100644
index 0000000..c887499
--- /dev/null
+++ b/y2024/y2024.json
@@ -0,0 +1,18 @@
+{
+ "channel_storage_duration": 10000000000,
+ "maps": [
+ {
+ "match": {
+ "name": "/aos",
+ "type": "aos.RobotState"
+ },
+ "rename": {
+ "name": "/roborio/aos"
+ }
+ }
+ ],
+ "imports": [
+ "y2024_roborio.json",
+ "y2024_imu.json"
+ ]
+}
diff --git a/y2024/y2024_imu.json b/y2024/y2024_imu.json
new file mode 100644
index 0000000..d2b5faa
--- /dev/null
+++ b/y2024/y2024_imu.json
@@ -0,0 +1,253 @@
+{
+ "channels": [
+ {
+ "name": "/imu/aos",
+ "type": "aos.JoystickState",
+ "source_node": "imu",
+ "frequency": 100
+ },
+ {
+ "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_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": "/imu/constants",
+ "type": "y2024.Constants",
+ "source_node": "imu",
+ "frequency": 1,
+ "num_senders": 2,
+ "max_size": 65536
+ }
+ ],
+ "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/y2024_roborio.json b/y2024/y2024_roborio.json
new file mode 100644
index 0000000..93caacc
--- /dev/null
+++ b/y2024/y2024_roborio.json
@@ -0,0 +1,453 @@
+{
+ "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": "/superstructure",
+ "type": "y2024.control_loops.superstructure.Goal",
+ "source_node": "roborio",
+ "frequency": 250,
+ "max_size": 512
+ },
+ {
+ "name": "/superstructure",
+ "type": "y2024.control_loops.superstructure.Status",
+ "source_node": "roborio",
+ "frequency": 400,
+ "num_senders": 2
+ },
+ {
+ "name": "/superstructure",
+ "type": "y2024.control_loops.superstructure.Output",
+ "source_node": "roborio",
+ "frequency": 250,
+ "num_senders": 2,
+ "max_size": 224
+ },
+ {
+ "name": "/superstructure",
+ "type": "y2024.control_loops.superstructure.Position",
+ "source_node": "roborio",
+ "frequency": 250,
+ "num_senders": 2,
+ "max_size": 448
+ },
+ {
+ "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",
+ "type": "frc971.CANConfiguration",
+ "source_node": "roborio",
+ "frequency": 2
+ },
+ {
+ "name": "/roborio/constants",
+ "type": "y2024.Constants",
+ "source_node": "roborio",
+ "frequency": 1,
+ "num_senders": 2,
+ "max_size": 65536
+ }
+ ],
+ "applications": [
+ {
+ "name": "drivetrain",
+ "executable_name": "drivetrain",
+ "nodes": [
+ "roborio"
+ ]
+ },
+ {
+ "name": "trajectory_generator",
+ "executable_name": "trajectory_generator",
+ "nodes": [
+ "roborio"
+ ]
+ },
+ {
+ "name": "superstructure",
+ "executable_name": "superstructure",
+ "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": "autonomous_action",
+ "executable_name": "autonomous_action",
+ "args": [
+ "--nodie_on_malloc"
+ ],
+ "autostart": true,
+ "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": "constants_sender_roborio",
+ "executable_name": "constants_sender",
+ "autorestart": false,
+ "nodes": [
+ "roborio"
+ ]
+ },
+ {
+ "name": "can_logger",
+ "executable_name": "can_logger",
+ "nodes": [
+ "roborio"
+ ]
+ }
+ ],
+ "maps": [
+ {
+ "match": {
+ "name": "/constants*",
+ "source_node": "roborio"
+ },
+ "rename": {
+ "name": "/roborio/constants"
+ }
+ },
+ {
+ "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-9971-FRC"
+ ],
+ "port": 9971
+ },
+ {
+ "name": "imu"
+ }
+ ]
+}