commit 0e37f59ce4d78fdcc64f7a46e5e453c18b919f2a
author Siddhant Kanwar <kanwarsiddhant@gmail.com> Mon Feb 21 19:26:50 2022 -0800
committer Austin Schuh <austin.linux@gmail.com> Mon Feb 21 21:42:36 2022 -0800
tree bc04607ad7397030a25144e7fff580e95c9a525d
parent 039d75607dee56e3dc35b39a860702d7f583d6b6

Add climber code without tests

This runs a loop to follow the request.

Signed-off-by: Siddhant Kanwar <kanwarsiddhant@gmail.com>
Change-Id: Iad516390561175f74eb33e0cda6a505376a372ed

y2022/control_loops/superstructure/superstructure.cc

diff --git a/y2022/control_loops/superstructure/superstructure.cc b/y2022/control_loops/superstructure/superstructure.cc
index b2c89d1..a7af922 100644
--- a/y2022/control_loops/superstructure/superstructure.cc
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -8,36 +8,44 @@
 
 using frc971::control_loops::AbsoluteEncoderProfiledJointStatus;
 using frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus;
+using frc971::control_loops:: RelativeEncoderProfiledJointStatus;
 
 Superstructure::Superstructure(::aos::EventLoop *event_loop,
                                const ::std::string &name)
     : frc971::controls::ControlLoop<Goal, Position, Status, Output>(event_loop,
-                                                                    name) {
+                                                                    name),
+      climber_(constants::GetValues().climber.subsystem_params) {
   event_loop->SetRuntimeRealtimePriority(30);
 }
 
 void Superstructure::RunIteration(const Goal *unsafe_goal,
-                                  const Position * /*position*/,
+                                  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_struct.climber_voltage = unsafe_goal->climber_speed();
+  OutputT output_struct;
+
+  flatbuffers::Offset<RelativeEncoderProfiledJointStatus>
+      climber_status_offset = climber_.Iterate(
+          unsafe_goal != nullptr ? unsafe_goal->climber() : nullptr,
+          position->climber(),
+          output != nullptr ? &(output_struct.climber_voltage) : nullptr,
+          status->fbb());
+
+  if (output != nullptr) {
     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);
+  // Climber is always zeroed; only has a pot
+  status_builder.add_zeroed(climber_.zeroed());
+  status_builder.add_estopped(climber_.estopped());
+  status_builder.add_climber(climber_status_offset);
 
-  if (unsafe_goal != nullptr) {
-    status_builder.add_climber_speed(unsafe_goal->climber_speed());
-  }
   (void)status->Send(status_builder.Finish());
 }