Add y2023 folder
2022 Bot specific things were removed
Change-Id: I6563d477a65bf2eae5d39933003742bd372cf82e
Signed-off-by: Maxwell Henderson <mxwhenderson@gmail.com>
Signed-off-by: Xander Yee <xander.yee@gmail.com>
diff --git a/y2023/control_loops/superstructure/BUILD b/y2023/control_loops/superstructure/BUILD
new file mode 100644
index 0000000..5246b3e
--- /dev/null
+++ b/y2023/control_loops/superstructure/BUILD
@@ -0,0 +1,145 @@
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("@com_github_google_flatbuffers//:typescript.bzl", "flatbuffer_ts_library")
+load("@npm//@bazel/typescript:index.bzl", "ts_library")
+
+package(default_visibility = ["//visibility:public"])
+
+flatbuffer_cc_library(
+ name = "superstructure_goal_fbs",
+ srcs = [
+ "superstructure_goal.fbs",
+ ],
+ gen_reflections = 1,
+ includes = [
+ "//frc971/control_loops:control_loops_fbs_includes",
+ "//frc971/control_loops:profiled_subsystem_fbs_includes",
+ ],
+)
+
+flatbuffer_cc_library(
+ name = "superstructure_output_fbs",
+ srcs = [
+ "superstructure_output.fbs",
+ ],
+ gen_reflections = 1,
+)
+
+flatbuffer_cc_library(
+ name = "superstructure_status_fbs",
+ srcs = [
+ "superstructure_status.fbs",
+ ],
+ gen_reflections = 1,
+ includes = [
+ "//frc971/control_loops:control_loops_fbs_includes",
+ "//frc971/control_loops:profiled_subsystem_fbs_includes",
+ ],
+)
+
+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",
+ ],
+)
+
+flatbuffer_cc_library(
+ name = "superstructure_position_fbs",
+ srcs = [
+ "superstructure_position.fbs",
+ ],
+ gen_reflections = 1,
+ includes = [
+ "//frc971/control_loops:control_loops_fbs_includes",
+ "//frc971/control_loops:profiled_subsystem_fbs_includes",
+ ],
+)
+
+cc_library(
+ name = "superstructure_lib",
+ srcs = [
+ "superstructure.cc",
+ ],
+ hdrs = [
+ "superstructure.h",
+ ],
+ deps = [
+ # ":collision_avoidance_lib",
+ ":superstructure_goal_fbs",
+ ":superstructure_output_fbs",
+ ":superstructure_position_fbs",
+ ":superstructure_status_fbs",
+ "//aos:flatbuffer_merge",
+ "//aos/events:event_loop",
+ "//frc971/control_loops:control_loop",
+ "//frc971/control_loops/drivetrain:drivetrain_status_fbs",
+ "//y2023:constants",
+ ],
+)
+
+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 = [
+ "//y2023:aos_config",
+ ],
+ deps = [
+ ":superstructure_goal_fbs",
+ ":superstructure_lib",
+ ":superstructure_output_fbs",
+ ":superstructure_position_fbs",
+ ":superstructure_status_fbs",
+ "//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_library(
+ 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/y2023/control_loops/superstructure/superstructure.cc b/y2023/control_loops/superstructure/superstructure.cc
new file mode 100644
index 0000000..1125e21
--- /dev/null
+++ b/y2023/control_loops/superstructure/superstructure.cc
@@ -0,0 +1,66 @@
+#include "y2023/control_loops/superstructure/superstructure.h"
+
+#include "aos/events/event_loop.h"
+#include "aos/flatbuffer_merge.h"
+#include "aos/network/team_number.h"
+#include "frc971/zeroing/wrap.h"
+
+DEFINE_bool(ignore_distance, false,
+ "If true, ignore distance when shooting and obay joystick_reader");
+
+namespace y2023 {
+namespace control_loops {
+namespace superstructure {
+
+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),
+ drivetrain_status_fetcher_(
+ event_loop->MakeFetcher<frc971::control_loops::drivetrain::Status>(
+ "/drivetrain")),
+ joystick_state_fetcher_(
+ event_loop->MakeFetcher<aos::JoystickState>("/aos")) {
+ (void)values;
+}
+
+void Superstructure::RunIteration(const Goal *unsafe_goal,
+ const Position *position,
+ aos::Sender<Output>::Builder *output,
+ aos::Sender<Status>::Builder *status) {
+ (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();
+ 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 y2023
diff --git a/y2023/control_loops/superstructure/superstructure.h b/y2023/control_loops/superstructure/superstructure.h
new file mode 100644
index 0000000..0740de3
--- /dev/null
+++ b/y2023/control_loops/superstructure/superstructure.h
@@ -0,0 +1,57 @@
+#ifndef Y2023_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
+#define Y2023_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
+
+#include "aos/events/event_loop.h"
+#include "frc971/control_loops/control_loop.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
+#include "y2023/constants.h"
+#include "y2023/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2023/control_loops/superstructure/superstructure_output_generated.h"
+#include "y2023/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2023/control_loops/superstructure/superstructure_status_generated.h"
+
+namespace y2023 {
+namespace control_loops {
+namespace superstructure {
+
+class Superstructure
+ : public ::frc971::controls::ControlLoop<Goal, Position, Status, Output> {
+ public:
+ using RelativeEncoderSubsystem =
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystem<
+ ::frc971::zeroing::RelativeEncoderZeroingEstimator,
+ ::frc971::control_loops::RelativeEncoderProfiledJointStatus>;
+
+ using PotAndAbsoluteEncoderSubsystem =
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystem<
+ ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator,
+ ::frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>;
+
+ 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_;
+
+ 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 y2023
+
+#endif // Y2023_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
diff --git a/y2023/control_loops/superstructure/superstructure_goal.fbs b/y2023/control_loops/superstructure/superstructure_goal.fbs
new file mode 100644
index 0000000..412abbd
--- /dev/null
+++ b/y2023/control_loops/superstructure/superstructure_goal.fbs
@@ -0,0 +1,11 @@
+include "frc971/control_loops/profiled_subsystem.fbs";
+
+namespace y2023.control_loops.superstructure;
+
+
+table Goal {
+}
+
+
+
+root_type Goal;
diff --git a/y2023/control_loops/superstructure/superstructure_lib_test.cc b/y2023/control_loops/superstructure/superstructure_lib_test.cc
new file mode 100644
index 0000000..0b9fb7d
--- /dev/null
+++ b/y2023/control_loops/superstructure/superstructure_lib_test.cc
@@ -0,0 +1,295 @@
+#include <chrono>
+#include <memory>
+
+#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 "gtest/gtest.h"
+#include "y2023/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "y2023/control_loops/superstructure/superstructure.h"
+
+DEFINE_string(output_folder, "",
+ "If set, logs all channels to the provided logfile.");
+
+namespace y2023 {
+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;
+typedef Superstructure::PotAndAbsoluteEncoderSubsystem
+ PotAndAbsoluteEncoderSubsystem;
+typedef Superstructure::RelativeEncoderSubsystem RelativeEncoderSubsystem;
+using DrivetrainStatus = ::frc971::control_loops::drivetrain::Status;
+using PotAndAbsoluteEncoderSimulator =
+ frc971::control_loops::SubsystemSimulator<
+ frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus,
+ PotAndAbsoluteEncoderSubsystem::State,
+ constants::Values::PotAndAbsEncoderConstants>;
+using RelativeEncoderSimulator = frc971::control_loops::SubsystemSimulator<
+ frc971::control_loops::RelativeEncoderProfiledJointStatus,
+ RelativeEncoderSubsystem::State, constants::Values::PotConstants>;
+
+// Class which simulates the superstructure and sends out queue messages with
+// the position.
+class SuperstructureSimulation {
+ public:
+ SuperstructureSimulation(::aos::EventLoop *event_loop,
+ std::shared_ptr<const constants::Values> values,
+ 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)values;
+ (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("y2023/aos_config.json"),
+ std::chrono::microseconds(5050)),
+ values_(std::make_shared<constants::Values>(constants::MakeValues())),
+ 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(), values_,
+ 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();
+ }
+
+ 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 aos::Node *const roborio_;
+ const aos::Node *const logger_pi_;
+
+ ::std::unique_ptr<::aos::EventLoop> superstructure_event_loop;
+ ::y2023::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_;
+}; // namespace testing
+
+// 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);
+ }
+
+ // Intake needs over 9 seconds to reach the goal
+ // 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 y2023
diff --git a/y2023/control_loops/superstructure/superstructure_main.cc b/y2023/control_loops/superstructure/superstructure_main.cc
new file mode 100644
index 0000000..bb25ad8
--- /dev/null
+++ b/y2023/control_loops/superstructure/superstructure_main.cc
@@ -0,0 +1,21 @@
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "y2023/control_loops/superstructure/superstructure.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());
+ std::shared_ptr<const y2023::constants::Values> values =
+ std::make_shared<const y2023::constants::Values>(
+ y2023::constants::MakeValues());
+ ::y2023::control_loops::superstructure::Superstructure superstructure(
+ &event_loop, values);
+
+ event_loop.Run();
+
+ return 0;
+}
diff --git a/y2023/control_loops/superstructure/superstructure_output.fbs b/y2023/control_loops/superstructure/superstructure_output.fbs
new file mode 100644
index 0000000..43de3d2
--- /dev/null
+++ b/y2023/control_loops/superstructure/superstructure_output.fbs
@@ -0,0 +1,6 @@
+namespace y2023.control_loops.superstructure;
+
+table Output {
+}
+
+root_type Output;
diff --git a/y2023/control_loops/superstructure/superstructure_plotter.ts b/y2023/control_loops/superstructure/superstructure_plotter.ts
new file mode 100644
index 0000000..ae20119
--- /dev/null
+++ b/y2023/control_loops/superstructure/superstructure_plotter.ts
@@ -0,0 +1,115 @@
+// Provides a plot for debugging robot state-related issues.
+import {AosPlotter} from 'org_frc971/aos/network/www/aos_plotter';
+import {BLUE, BROWN, CYAN, GREEN, PINK, RED, WHITE} from 'org_frc971/aos/network/www/colors';
+import * as proxy from 'org_frc971/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', 'y2023.control_loops.superstructure.Goal');
+ const output = aosPlotter.addMessageSource(
+ '/superstructure', 'y2023.control_loops.superstructure.Output');
+ const status = aosPlotter.addMessageSource(
+ '/superstructure', 'y2023.control_loops.superstructure.Status');
+ const position = aosPlotter.addMessageSource(
+ '/superstructure', 'y2023.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]);
+
+ positionPlot.addMessageLine(position, ['turret_beambreak'])
+ .setColor(RED)
+ .setPointSize(4.0);
+ positionPlot.addMessageLine(status, ['state'])
+ .setColor(PINK)
+ .setPointSize(4.0);
+ positionPlot.addMessageLine(status, ['flippers_open'])
+ .setColor(WHITE)
+ .setPointSize(1.0);
+ positionPlot.addMessageLine(status, ['reseating_in_catapult'])
+ .setColor(BLUE)
+ .setPointSize(1.0);
+ positionPlot.addMessageLine(status, ['fire'])
+ .setColor(BROWN)
+ .setPointSize(1.0);
+ positionPlot.addMessageLine(status, ['ready_to_fire'])
+ .setColor(GREEN)
+ .setPointSize(1.0);
+ positionPlot.addMessageLine(status, ['collided'])
+ .setColor(PINK)
+ .setPointSize(1.0);
+
+ const goalPlot =
+ aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
+ goalPlot.plot.getAxisLabels().setTitle('Goal');
+ goalPlot.plot.getAxisLabels().setXLabel(TIME);
+ goalPlot.plot.getAxisLabels().setYLabel('value');
+ goalPlot.plot.setDefaultYRange([-1.0, 2.0]);
+ goalPlot.addMessageLine(goal, ['fire']).setColor(RED).setPointSize(1.0);
+ goalPlot.addMessageLine(goal, ['auto_aim']).setColor(BLUE).setPointSize(1.0);
+
+
+ const shotCountPlot =
+ aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
+ shotCountPlot.plot.getAxisLabels().setTitle('Shot Count');
+ shotCountPlot.plot.getAxisLabels().setXLabel(TIME);
+ shotCountPlot.plot.getAxisLabels().setYLabel('balls');
+ shotCountPlot.plot.setDefaultYRange([-1.0, 2.0]);
+ shotCountPlot.addMessageLine(status, ['shot_count'])
+ .setColor(RED)
+ .setPointSize(1.0);
+
+ const intakePlot =
+ aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
+ intakePlot.plot.getAxisLabels().setTitle('Intake');
+ intakePlot.plot.getAxisLabels().setXLabel(TIME);
+ intakePlot.plot.getAxisLabels().setYLabel('wonky state units');
+ intakePlot.plot.setDefaultYRange([-1.0, 2.0]);
+ intakePlot.addMessageLine(status, ['intake_state'])
+ .setColor(RED)
+ .setPointSize(1.0);
+ intakePlot.addMessageLine(position, ['intake_beambreak_front'])
+ .setColor(GREEN)
+ .setPointSize(4.0);
+ intakePlot.addMessageLine(position, ['intake_beambreak_back'])
+ .setColor(PINK)
+ .setPointSize(1.0);
+ intakePlot.addMessageLine(output, ['transfer_roller_voltage'])
+ .setColor(BROWN)
+ .setPointSize(3.0);
+
+
+ const otherPlot =
+ aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
+ otherPlot.plot.getAxisLabels().setTitle('Position');
+ otherPlot.plot.getAxisLabels().setXLabel(TIME);
+ otherPlot.plot.getAxisLabels().setYLabel('rad');
+ otherPlot.plot.setDefaultYRange([-1.0, 2.0]);
+
+ otherPlot.addMessageLine(status, ['catapult', 'position'])
+ .setColor(PINK)
+ .setPointSize(4.0);
+ otherPlot.addMessageLine(status, ['turret', 'position'])
+ .setColor(WHITE)
+ .setPointSize(4.0);
+ otherPlot.addMessageLine(position, ['flipper_arm_left', 'encoder'])
+ .setColor(BLUE)
+ .setPointSize(4.0);
+ otherPlot.addMessageLine(position, ['flipper_arm_right', 'encoder'])
+ .setColor(CYAN)
+ .setPointSize(4.0);
+ otherPlot.addMessageLine(output, ['flipper_arms_voltage'])
+ .setColor(BROWN)
+ .setPointSize(4.0);
+}
diff --git a/y2023/control_loops/superstructure/superstructure_position.fbs b/y2023/control_loops/superstructure/superstructure_position.fbs
new file mode 100644
index 0000000..49abd28
--- /dev/null
+++ b/y2023/control_loops/superstructure/superstructure_position.fbs
@@ -0,0 +1,8 @@
+include "frc971/control_loops/control_loops.fbs";
+
+namespace y2023.control_loops.superstructure;
+
+table Position {
+}
+
+root_type Position;
diff --git a/y2023/control_loops/superstructure/superstructure_replay.cc b/y2023/control_loops/superstructure/superstructure_replay.cc
new file mode 100644
index 0000000..ed97966
--- /dev/null
+++ b/y2023/control_loops/superstructure/superstructure_replay.cc
@@ -0,0 +1,74 @@
+// 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 "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 "gflags/gflags.h"
+#include "y2023/constants.h"
+#include "y2023/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",
+ "y2023.control_loops.superstructure.Status");
+ reader.RemapLoggedChannel("/superstructure",
+ "y2023.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<y2023::control_loops::superstructure::Superstructure>(
+ "superstructure", std::make_shared<y2023::constants::Values>(
+ y2023::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 y2023::control_loops::superstructure::Status &status) {
+ if (status.estopped()) {
+ LOG(ERROR) << "Estopped";
+ }
+ });
+
+ factory.Run();
+
+ reader.Deregister();
+
+ return 0;
+}
diff --git a/y2023/control_loops/superstructure/superstructure_status.fbs b/y2023/control_loops/superstructure/superstructure_status.fbs
new file mode 100644
index 0000000..ef9667d
--- /dev/null
+++ b/y2023/control_loops/superstructure/superstructure_status.fbs
@@ -0,0 +1,15 @@
+include "frc971/control_loops/control_loops.fbs";
+include "frc971/control_loops/profiled_subsystem.fbs";
+
+namespace y2023.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;