commit 0f2d38c53a0d00c05fae3e8b272a77026132bad5
author Sabina Davis <sabina1109@gmail.com> Sat Feb 08 17:01:21 2020 -0800
committer Sabina Davis <sabina1109@gmail.com> Mon Feb 17 16:41:43 2020 -0800
tree a03ce9c8a128e14f2e349c2cdeb0e1e149c4b53d
parent e8d38997f8d294493b136c76b1a836e7615bf9d6

Added intake and joint to superstructure and tests.

Change-Id: Ibdf15b6bade03c1bd21113490c5c4d12e7edadf9

y2020/control_loops/superstructure/superstructure.cc

diff --git a/y2020/control_loops/superstructure/superstructure.cc b/y2020/control_loops/superstructure/superstructure.cc
index 91a7586..cfa6d94 100644
--- a/y2020/control_loops/superstructure/superstructure.cc
+++ b/y2020/control_loops/superstructure/superstructure.cc
@@ -13,7 +13,8 @@
                                const ::std::string &name)
     : aos::controls::ControlLoop<Goal, Position, Status, Output>(event_loop,
                                                                  name),
-      hood_(constants::GetValues().hood) {
+      hood_(constants::GetValues().hood),
+      intake_joint_(constants::GetValues().intake) {
   event_loop->SetRuntimeRealtimePriority(30);
 }
 
@@ -24,6 +25,7 @@
   if (WasReset()) {
     AOS_LOG(ERROR, "WPILib reset, restarting\n");
     hood_.Reset();
+    intake_joint_.Reset();
   }
 
   OutputT output_struct;
@@ -34,16 +36,25 @@
                     output != nullptr ? &(output_struct.hood_voltage) : nullptr,
                     status->fbb());
 
+  flatbuffers::Offset<AbsoluteEncoderProfiledJointStatus> intake_status_offset =
+      intake_joint_.Iterate(
+          unsafe_goal != nullptr ? unsafe_goal->intake() : nullptr,
+          position->intake_joint(),
+          output != nullptr ? &(output_struct.intake_joint_voltage) : nullptr,
+          status->fbb());
+
   bool zeroed;
   bool estopped;
 
-  const AbsoluteEncoderProfiledJointStatus *hood_status =
-      GetMutableTemporaryPointer(*status->fbb(), hood_status_offset);
-  zeroed = hood_status->zeroed();
-  estopped = hood_status->estopped();
+  {
+    AbsoluteEncoderProfiledJointStatus *hood_status =
+        GetMutableTemporaryPointer(*status->fbb(), hood_status_offset);
 
-  if (output != nullptr) {
-    output->Send(Output::Pack(*output->fbb(), &output_struct));
+    AbsoluteEncoderProfiledJointStatus *intake_status =
+        GetMutableTemporaryPointer(*status->fbb(), intake_status_offset);
+
+    zeroed = hood_status->zeroed() && intake_status->zeroed();
+    estopped = hood_status->estopped() || intake_status->estopped();
   }
 
   Status::Builder status_builder = status->MakeBuilder<Status>();
@@ -52,8 +63,18 @@
   status_builder.add_estopped(estopped);
 
   status_builder.add_hood(hood_status_offset);
+  status_builder.add_intake(intake_status_offset);
 
   status->Send(status_builder.Finish());
+
+  if (output != nullptr) {
+    if (unsafe_goal) {
+      output_struct.intake_roller_voltage = unsafe_goal->roller_voltage();
+    } else {
+      output_struct.intake_roller_voltage = 0.0;
+    }
+    output->Send(Output::Pack(*output->fbb(), &output_struct));
+  }
 }
 
 }  // namespace superstructure