Create y2022 directory
Signed-off-by: milind-u <milind.upadhyay@gmail.com>
Change-Id: I3a57026587204e084276c7e338c397467f776a70
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