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/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;