Add y2024 folder
2023 bot specific code was removed.
Signed-off-by: Nathan Leong <100028864@mvla.net>
Change-Id: I88fc4a4b5e6bc883ea327cc306efa4e20035908b
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;