commit fb460377a72761444997b2a676c8d11ded5b3274
author Kai Tinkess <100021925@mvla.net> Sat Feb 08 14:05:48 2020 -0800
committer Austin Schuh <austin.linux@gmail.com> Mon Feb 17 17:41:44 2020 -0800
tree f868e626a649eb77c577d27df56da4a2a4090365
parent 10943cfe0f493e366fa53f7b525865afaf7e8280

Add Turret to main superstructure loop and tests

Change-Id: Ie2d2936aee82126f7d3ad453c65ccc82e06cee78

y2020/control_loops/superstructure/superstructure.cc

diff --git a/y2020/control_loops/superstructure/superstructure.cc b/y2020/control_loops/superstructure/superstructure.cc
index cfa6d94..eb88e93 100644
--- a/y2020/control_loops/superstructure/superstructure.cc
+++ b/y2020/control_loops/superstructure/superstructure.cc
@@ -14,7 +14,8 @@
     : aos::controls::ControlLoop<Goal, Position, Status, Output>(event_loop,
                                                                  name),
       hood_(constants::GetValues().hood),
-      intake_joint_(constants::GetValues().intake) {
+      intake_joint_(constants::GetValues().intake),
+      turret_(constants::GetValues().turret.subsystem_params) {
   event_loop->SetRuntimeRealtimePriority(30);
 }
 
@@ -26,6 +27,7 @@
     AOS_LOG(ERROR, "WPILib reset, restarting\n");
     hood_.Reset();
     intake_joint_.Reset();
+    turret_.Reset();
   }
 
   OutputT output_struct;
@@ -43,18 +45,30 @@
           output != nullptr ? &(output_struct.intake_joint_voltage) : nullptr,
           status->fbb());
 
+  flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
+      turret_status_offset = turret_.Iterate(
+          unsafe_goal != nullptr ? unsafe_goal->turret() : nullptr,
+          position->turret(),
+          output != nullptr ? &(output_struct.turret_voltage) : nullptr,
+          status->fbb());
+
   bool zeroed;
   bool estopped;
 
   {
-    AbsoluteEncoderProfiledJointStatus *hood_status =
+    const AbsoluteEncoderProfiledJointStatus *const hood_status =
         GetMutableTemporaryPointer(*status->fbb(), hood_status_offset);
 
-    AbsoluteEncoderProfiledJointStatus *intake_status =
+    const AbsoluteEncoderProfiledJointStatus *const intake_status =
         GetMutableTemporaryPointer(*status->fbb(), intake_status_offset);
 
-    zeroed = hood_status->zeroed() && intake_status->zeroed();
-    estopped = hood_status->estopped() || intake_status->estopped();
+    const PotAndAbsoluteEncoderProfiledJointStatus *const turret_status =
+        GetMutableTemporaryPointer(*status->fbb(), turret_status_offset);
+
+    zeroed = hood_status->zeroed() && intake_status->zeroed() &&
+             turret_status->zeroed();
+    estopped = hood_status->estopped() || intake_status->estopped() ||
+               turret_status->estopped();
   }
 
   Status::Builder status_builder = status->MakeBuilder<Status>();
@@ -64,6 +78,7 @@
 
   status_builder.add_hood(hood_status_offset);
   status_builder.add_intake(intake_status_offset);
+  status_builder.add_turret(turret_status_offset);
 
   status->Send(status_builder.Finish());