Add y2023_bot3 folder
Signed-off-by: James (Peilun) Li <jamespeilunli@gmail.com>
Change-Id: I6fae2492d0497596a632efc79a6a258c0ca15f49
diff --git a/y2023_bot3/control_loops/superstructure/superstructure.cc b/y2023_bot3/control_loops/superstructure/superstructure.cc
new file mode 100644
index 0000000..e9df534
--- /dev/null
+++ b/y2023_bot3/control_loops/superstructure/superstructure.cc
@@ -0,0 +1,61 @@
+#include "y2023_bot3/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 y2023_bot3 {
+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) {
+ event_loop->SetRuntimeRealtimePriority(30);
+}
+
+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;
+
+ const monotonic_clock::time_point timestamp =
+ event_loop()->context().monotonic_event_time;
+ (void)timestamp;
+
+ if (WasReset()) {
+ AOS_LOG(ERROR, "WPILib reset, restarting\n");
+ }
+
+ OutputT output_struct;
+
+ if (output) {
+ output->CheckOk(output->Send(Output::Pack(*output->fbb(), &output_struct)));
+ }
+
+ Status::Builder status_builder = status->MakeBuilder<Status>();
+ status_builder.add_zeroed(true);
+
+ (void)status->Send(status_builder.Finish());
+}
+
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2023_bot3