commit 0d2207768f51d70ff8055de595774b6ee0b88131
author Maxwell Henderson <maxwell.henderson@mailbox.org> Mon Nov 06 11:09:58 2023 -0800
committer Maxwell Henderson <maxwell.henderson@mailbox.org> Wed Nov 08 12:33:32 2023 -0800
tree d20ff08fb0f4ff1cff2909e1e80a5d318fbce47a
parent 43684fa4ac36692de0be88035b806a7b41ae8abb

Add pivot joint functionality

Signed-off-by: Charlie Huang <charliehuang09@gmail.com>
Change-Id: Ib40d4ce1a4c91f1b0e75b3548fe43c9218433634

y2023_bot3/control_loops/superstructure/superstructure.cc

diff --git a/y2023_bot3/control_loops/superstructure/superstructure.cc b/y2023_bot3/control_loops/superstructure/superstructure.cc
index 3e88a6c..3b4835c 100644
--- a/y2023_bot3/control_loops/superstructure/superstructure.cc
+++ b/y2023_bot3/control_loops/superstructure/superstructure.cc
@@ -23,7 +23,9 @@
                                const ::std::string &name)
     : frc971::controls::ControlLoop<Goal, Position, Status, Output>(event_loop,
                                                                     name),
-      values_(values) {
+      values_(values),
+      end_effector_(),
+      pivot_joint_(values) {
   event_loop->SetRuntimeRealtimePriority(30);
 }
 
@@ -47,15 +49,25 @@
       position->end_effector_cube_beam_break(), &output_struct.roller_voltage,
       unsafe_goal != nullptr ? unsafe_goal->preloaded_with_cube() : false);
 
-  if (output) {
-    output->CheckOk(output->Send(Output::Pack(*output->fbb(), &output_struct)));
-  }
+  flatbuffers::Offset<
+      frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>
+      pivot_joint_offset = pivot_joint_.RunIteration(
+          unsafe_goal != nullptr ? unsafe_goal->pivot_goal()
+                                 : PivotGoal::NEUTRAL,
+          &(output_struct.pivot_joint_voltage),
+          position->pivot_joint_position(), status->fbb());
 
   Status::Builder status_builder = status->MakeBuilder<Status>();
 
-  status_builder.add_zeroed(true);
+  status_builder.add_zeroed(pivot_joint_.zeroed());
+  status_builder.add_estopped(pivot_joint_.estopped());
+  status_builder.add_pivot_joint(pivot_joint_offset);
   status_builder.add_end_effector_state(end_effector_.state());
 
+  if (output) {
+    output->CheckOk(output->Send(Output::Pack(*output->fbb(), &output_struct)));
+  }
+
   (void)status->Send(status_builder.Finish());
 }