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/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