Create y2022 directory
Signed-off-by: milind-u <milind.upadhyay@gmail.com>
Change-Id: I3a57026587204e084276c7e338c397467f776a70
diff --git a/y2022/control_loops/drivetrain/BUILD b/y2022/control_loops/drivetrain/BUILD
new file mode 100644
index 0000000..2858dc7
--- /dev/null
+++ b/y2022/control_loops/drivetrain/BUILD
@@ -0,0 +1,85 @@
+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 //y2022/control_loops/python:drivetrain) $(OUTS)",
+ target_compatible_with = ["@platforms//os:linux"],
+ tools = [
+ "//y2022/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 //y2022/control_loops/python:polydrivetrain) $(OUTS)",
+ target_compatible_with = ["@platforms//os:linux"],
+ tools = [
+ "//y2022/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/control_loops/drivetrain:drivetrain_lib",
+ ],
+)
diff --git a/y2022/control_loops/drivetrain/drivetrain_base.cc b/y2022/control_loops/drivetrain/drivetrain_base.cc
new file mode 100644
index 0000000..cbc9c42
--- /dev/null
+++ b/y2022/control_loops/drivetrain/drivetrain_base.cc
@@ -0,0 +1,63 @@
+#include "y2022/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 "y2022/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "y2022/control_loops/drivetrain/hybrid_velocity_drivetrain.h"
+#include "y2022/control_loops/drivetrain/kalman_drivetrain_motor_plant.h"
+#include "y2022/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
+
+using ::frc971::control_loops::drivetrain::DrivetrainConfig;
+
+namespace chrono = ::std::chrono;
+
+namespace y2022 {
+namespace control_loops {
+namespace drivetrain {
+
+using ::frc971::constants::ShifterHallEffect;
+
+const ShifterHallEffect kThreeStateDriveShifter{0.0, 0.0, 0.25, 0.75};
+
+const DrivetrainConfig<double> &GetDrivetrainConfig() {
+ static DrivetrainConfig<double> kDrivetrainConfig{
+ ::frc971::control_loops::drivetrain::ShifterType::SIMPLE_SHIFTER,
+ ::frc971::control_loops::drivetrain::LoopType::CLOSED_LOOP,
+ ::frc971::control_loops::drivetrain::GyroType::IMU_Z_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*/,
+ };
+
+ return kDrivetrainConfig;
+};
+
+} // namespace drivetrain
+} // namespace control_loops
+} // namespace y2022
diff --git a/y2022/control_loops/drivetrain/drivetrain_base.h b/y2022/control_loops/drivetrain/drivetrain_base.h
new file mode 100644
index 0000000..2983016
--- /dev/null
+++ b/y2022/control_loops/drivetrain/drivetrain_base.h
@@ -0,0 +1,17 @@
+#ifndef Y2022_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_BASE_H_
+#define Y2022_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_BASE_H_
+
+#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+
+namespace y2022 {
+namespace control_loops {
+namespace drivetrain {
+
+const ::frc971::control_loops::drivetrain::DrivetrainConfig<double>
+ &GetDrivetrainConfig();
+
+} // namespace drivetrain
+} // namespace control_loops
+} // namespace y2022
+
+#endif // Y2022_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_BASE_H_
diff --git a/y2022/control_loops/drivetrain/drivetrain_main.cc b/y2022/control_loops/drivetrain/drivetrain_main.cc
new file mode 100644
index 0000000..e29f950
--- /dev/null
+++ b/y2022/control_loops/drivetrain/drivetrain_main.cc
@@ -0,0 +1,27 @@
+#include <memory>
+
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "frc971/control_loops/drivetrain/drivetrain.h"
+#include "y2022/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("config.json");
+
+ ::aos::ShmEventLoop event_loop(&config.message());
+ ::frc971::control_loops::drivetrain::DeadReckonEkf localizer(
+ &event_loop,
+ ::y2022::control_loops::drivetrain::GetDrivetrainConfig());
+ std::unique_ptr<DrivetrainLoop> drivetrain = std::make_unique<DrivetrainLoop>(
+ ::y2022::control_loops::drivetrain::GetDrivetrainConfig(),
+ &event_loop, &localizer);
+
+ event_loop.Run();
+
+ return 0;
+}
diff --git a/y2022/control_loops/python/BUILD b/y2022/control_loops/python/BUILD
new file mode 100644
index 0000000..e0b0daa
--- /dev/null
+++ b/y2022/control_loops/python/BUILD
@@ -0,0 +1,57 @@
+package(default_visibility = ["//y2022:__subpackages__"])
+
+py_binary(
+ name = "drivetrain",
+ srcs = [
+ "drivetrain.py",
+ ],
+ legacy_create_init = False,
+ target_compatible_with = ["@platforms//cpu:x86_64"],
+ deps = [
+ ":python_init",
+ "//external:python-gflags",
+ "//external:python-glog",
+ "//frc971/control_loops/python:drivetrain",
+ ],
+)
+
+py_binary(
+ name = "polydrivetrain",
+ srcs = [
+ "drivetrain.py",
+ "polydrivetrain.py",
+ ],
+ legacy_create_init = False,
+ target_compatible_with = ["@platforms//cpu:x86_64"],
+ deps = [
+ ":python_init",
+ "//external:python-gflags",
+ "//external:python-glog",
+ "//frc971/control_loops/python:polydrivetrain",
+ ],
+)
+
+py_library(
+ name = "polydrivetrain_lib",
+ srcs = [
+ "drivetrain.py",
+ "polydrivetrain.py",
+ ],
+ target_compatible_with = ["@platforms//cpu:x86_64"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//external:python-gflags",
+ "//external:python-glog",
+ "//frc971/control_loops/python:controls",
+ "//frc971/control_loops/python:drivetrain",
+ "//frc971/control_loops/python:polydrivetrain",
+ ],
+)
+
+py_library(
+ name = "python_init",
+ srcs = ["__init__.py"],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = ["//y2020/control_loops:python_init"],
+)
diff --git a/y2022/control_loops/python/__init__.py b/y2022/control_loops/python/__init__.py
new file mode 100644
index 0000000..e69de29
--- /dev/null
+++ b/y2022/control_loops/python/__init__.py
diff --git a/y2022/control_loops/python/drivetrain.py b/y2022/control_loops/python/drivetrain.py
new file mode 100644
index 0000000..7b8dc49
--- /dev/null
+++ b/y2022/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.0,
+ mass=58.0,
+ # TODO(austin): Measure radius a bit better.
+ robot_radius= 0.39,
+ wheel_radius= 3/39.37,
+ motor_type=control_loop.Falcon(),
+ num_motors = 3,
+ G=8.0 / 80.0,
+ q_pos=0.24,
+ q_vel=2.5,
+ efficiency=0.80,
+ has_imu=True,
+ 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], 'y2022', kDrivetrain)
+
+
+if __name__ == '__main__':
+ sys.exit(main(sys.argv))
diff --git a/y2022/control_loops/python/polydrivetrain.py b/y2022/control_loops/python/polydrivetrain.py
new file mode 100644
index 0000000..d749580
--- /dev/null
+++ b/y2022/control_loops/python/polydrivetrain.py
@@ -0,0 +1,31 @@
+#!/usr/bin/python3
+
+import sys
+from y2022.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], 'y2022',
+ drivetrain.kDrivetrain)
+
+if __name__ == '__main__':
+ argv = FLAGS(sys.argv)
+ glog.init()
+ sys.exit(main(argv))
diff --git a/y2022/control_loops/superstructure/BUILD b/y2022/control_loops/superstructure/BUILD
new file mode 100644
index 0000000..ccfb0ee
--- /dev/null
+++ b/y2022/control_loops/superstructure/BUILD
@@ -0,0 +1,116 @@
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_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_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 = [
+ ":superstructure_goal_fbs",
+ ":superstructure_output_fbs",
+ ":superstructure_position_fbs",
+ ":superstructure_status_fbs",
+ "//aos/events:event_loop",
+ "//frc971/control_loops:control_loop",
+ "//y2022: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 = [
+ "//y2022: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:team_number_test_environment",
+ "//frc971/control_loops/drivetrain:drivetrain_status_fbs",
+ ],
+)
+
+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/y2022/control_loops/superstructure/superstructure.cc b/y2022/control_loops/superstructure/superstructure.cc
new file mode 100644
index 0000000..e985779
--- /dev/null
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -0,0 +1,46 @@
+#include "y2022/control_loops/superstructure/superstructure.h"
+
+#include "aos/events/event_loop.h"
+
+namespace y2022 {
+namespace control_loops {
+namespace superstructure {
+
+using frc971::control_loops::AbsoluteEncoderProfiledJointStatus;
+using frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus;
+
+Superstructure::Superstructure(::aos::EventLoop *event_loop,
+ const ::std::string &name)
+ : frc971::controls::ControlLoop<Goal, Position, Status, Output>(event_loop,
+ name) {
+ event_loop->SetRuntimeRealtimePriority(30);
+}
+
+void Superstructure::RunIteration(const Goal *unsafe_goal,
+ const Position * /*position*/,
+ aos::Sender<Output>::Builder *output,
+ aos::Sender<Status>::Builder *status) {
+ if (WasReset()) {
+ AOS_LOG(ERROR, "WPILib reset, restarting\n");
+ }
+
+ if (output != nullptr && unsafe_goal != nullptr) {
+ OutputT output_struct;
+
+ 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);
+
+ if (unsafe_goal != nullptr) {
+ }
+
+ (void)status->Send(status_builder.Finish());
+}
+
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2022
diff --git a/y2022/control_loops/superstructure/superstructure.h b/y2022/control_loops/superstructure/superstructure.h
new file mode 100644
index 0000000..95e24d3
--- /dev/null
+++ b/y2022/control_loops/superstructure/superstructure.h
@@ -0,0 +1,35 @@
+#ifndef Y2022_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
+#define Y2022_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
+
+#include "frc971/control_loops/control_loop.h"
+#include "aos/events/event_loop.h"
+#include "y2022/constants.h"
+#include "y2022/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2022/control_loops/superstructure/superstructure_output_generated.h"
+#include "y2022/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2022/control_loops/superstructure/superstructure_status_generated.h"
+
+namespace y2022 {
+namespace control_loops {
+namespace superstructure {
+
+class Superstructure
+ : public ::frc971::controls::ControlLoop<Goal, Position, Status, Output> {
+ public:
+ explicit Superstructure(::aos::EventLoop *event_loop,
+ const ::std::string &name = "/superstructure");
+
+ protected:
+ virtual void RunIteration(const Goal *unsafe_goal, const Position *position,
+ aos::Sender<Output>::Builder *output,
+ aos::Sender<Status>::Builder *status) override;
+
+ private:
+ DISALLOW_COPY_AND_ASSIGN(Superstructure);
+};
+
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2022
+
+#endif // Y2022_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
diff --git a/y2022/control_loops/superstructure/superstructure_goal.fbs b/y2022/control_loops/superstructure/superstructure_goal.fbs
new file mode 100644
index 0000000..439c0fb
--- /dev/null
+++ b/y2022/control_loops/superstructure/superstructure_goal.fbs
@@ -0,0 +1,9 @@
+include "frc971/control_loops/profiled_subsystem.fbs";
+
+namespace y2022.control_loops.superstructure;
+
+table Goal {
+
+}
+
+root_type Goal;
diff --git a/y2022/control_loops/superstructure/superstructure_lib_test.cc b/y2022/control_loops/superstructure/superstructure_lib_test.cc
new file mode 100644
index 0000000..82c25b3
--- /dev/null
+++ b/y2022/control_loops/superstructure/superstructure_lib_test.cc
@@ -0,0 +1,83 @@
+#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/team_number_test_environment.h"
+#include "gtest/gtest.h"
+#include "y2022/control_loops/superstructure/superstructure.h"
+
+DEFINE_string(output_folder, "",
+ "If set, logs all channels to the provided logfile.");
+
+namespace y2022 {
+namespace control_loops {
+namespace superstructure {
+namespace testing {
+
+class SuperstructureTest : public ::frc971::testing::ControlLoopTest {
+ public:
+ SuperstructureTest()
+ : ::frc971::testing::ControlLoopTest(
+ aos::configuration::ReadConfig("y2022/config.json"),
+ std::chrono::microseconds(5050)),
+ superstructure_event_loop(MakeEventLoop("Superstructure")),
+ superstructure_(superstructure_event_loop.get()),
+ test_event_loop_(MakeEventLoop("test")),
+ 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")) {
+ set_team_id(frc971::control_loops::testing::kTeamNumber);
+ SetEnabled(true);
+
+ phased_loop_handle_ = test_event_loop_->AddPhasedLoop(
+ [this](int) { SendPositionMessage(); }, dt());
+
+ 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_->StartLoggingLocalNamerOnRun(FLAGS_output_folder);
+ }
+ }
+
+ void SendPositionMessage() {
+ auto builder = superstructure_position_sender_.MakeBuilder();
+ Position::Builder position_builder = builder.MakeBuilder<Position>();
+ builder.CheckOk(builder.Send(position_builder.Finish()));
+ }
+
+ // Because the third robot is single node, the roborio node is nullptr
+ const aos::Node *const roborio_ = nullptr;
+
+ ::std::unique_ptr<::aos::EventLoop> superstructure_event_loop;
+ ::y2022::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_;
+
+ std::unique_ptr<aos::EventLoop> logger_event_loop_;
+ std::unique_ptr<aos::logger::Logger> logger_;
+};
+
+} // namespace testing
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2022
diff --git a/y2022/control_loops/superstructure/superstructure_main.cc b/y2022/control_loops/superstructure/superstructure_main.cc
new file mode 100644
index 0000000..f53e042
--- /dev/null
+++ b/y2022/control_loops/superstructure/superstructure_main.cc
@@ -0,0 +1,19 @@
+#include "y2022/control_loops/superstructure/superstructure.h"
+
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+
+int main(int argc, char **argv) {
+ ::aos::InitGoogle(&argc, &argv);
+
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
+ ::aos::ShmEventLoop event_loop(&config.message());
+ ::y2022::control_loops::superstructure::Superstructure superstructure(
+ &event_loop);
+
+ event_loop.Run();
+
+ return 0;
+}
diff --git a/y2022/control_loops/superstructure/superstructure_output.fbs b/y2022/control_loops/superstructure/superstructure_output.fbs
new file mode 100644
index 0000000..e95fbe3
--- /dev/null
+++ b/y2022/control_loops/superstructure/superstructure_output.fbs
@@ -0,0 +1,7 @@
+namespace y2022.control_loops.superstructure;
+
+table Output {
+
+}
+
+root_type Output;
diff --git a/y2022/control_loops/superstructure/superstructure_plotter.ts b/y2022/control_loops/superstructure/superstructure_plotter.ts
new file mode 100644
index 0000000..1f1a4c5
--- /dev/null
+++ b/y2022/control_loops/superstructure/superstructure_plotter.ts
@@ -0,0 +1,19 @@
+// Provides a plot for debugging robot state-related issues.
+import {AosPlotter} from 'org_frc971/aos/network/www/aos_plotter';
+import * as proxy from 'org_frc971/aos/network/www/proxy';
+import {BLUE, BROWN, CYAN, GREEN, PINK, RED, WHITE} from 'org_frc971/aos/network/www/colors';
+
+import Connection = proxy.Connection;
+
+const TIME = AosPlotter.TIME;
+const DEFAULT_WIDTH = AosPlotter.DEFAULT_WIDTH;
+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', 'y2022.control_loops.superstructure.Goal');
+ const output = aosPlotter.addMessageSource('/superstructure', 'y2022.control_loops.superstructure.Output');
+ const status = aosPlotter.addMessageSource('/superstructure', 'y2022.control_loops.superstructure.Status');
+ const position = aosPlotter.addMessageSource('/superstructure', 'y2022.control_loops.superstructure.Position');
+ const robotState = aosPlotter.addMessageSource('/aos', 'aos.RobotState');
+}
diff --git a/y2022/control_loops/superstructure/superstructure_position.fbs b/y2022/control_loops/superstructure/superstructure_position.fbs
new file mode 100644
index 0000000..02002ba
--- /dev/null
+++ b/y2022/control_loops/superstructure/superstructure_position.fbs
@@ -0,0 +1,9 @@
+include "frc971/control_loops/control_loops.fbs";
+
+namespace y2022.control_loops.superstructure;
+
+table Position {
+
+}
+
+root_type Position;
diff --git a/y2022/control_loops/superstructure/superstructure_status.fbs b/y2022/control_loops/superstructure/superstructure_status.fbs
new file mode 100644
index 0000000..deff2f6
--- /dev/null
+++ b/y2022/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 y2022.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;
\ No newline at end of file