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