commit 0f31d3fb821c60e5dc4016b7251f9b2289e54c05
author Sabina Davis <sabina1109@gmail.com> Thu Feb 20 20:41:00 2020 -0800
committer Sabina Davis <sabina1109@gmail.com> Fri Feb 21 20:28:26 2020 -0800
tree 7c1c729aa0c7ee77fe7e7094895ce044fc09ac85
parent bd588a3def562906e9bbb41823020a039053a0ef

Added shooter and tests.

Added the shooter into the main superstructure and added tests.

Change-Id: I6c9afe3c74a08251854805050c40fafdca90fba8

y2020/control_loops/superstructure/superstructure.cc

diff --git a/y2020/control_loops/superstructure/superstructure.cc b/y2020/control_loops/superstructure/superstructure.cc
index c2512db..39c913e 100644
--- a/y2020/control_loops/superstructure/superstructure.cc
+++ b/y2020/control_loops/superstructure/superstructure.cc
@@ -15,7 +15,8 @@
                                                                  name),
       hood_(constants::GetValues().hood),
       intake_joint_(constants::GetValues().intake),
-      turret_(constants::GetValues().turret.subsystem_params) {
+      turret_(constants::GetValues().turret.subsystem_params),
+      shooter_() {
   event_loop->SetRuntimeRealtimePriority(30);
 }
 
@@ -30,6 +31,9 @@
     turret_.Reset();
   }
 
+  const aos::monotonic_clock::time_point position_timestamp =
+      event_loop()->context().monotonic_event_time;
+
   OutputT output_struct;
 
   flatbuffers::Offset<AbsoluteEncoderProfiledJointStatus> hood_status_offset =
@@ -52,6 +56,12 @@
           output != nullptr ? &(output_struct.turret_voltage) : nullptr,
           status->fbb());
 
+  flatbuffers::Offset<ShooterStatus> shooter_status_offset =
+      shooter_.RunIteration(
+          unsafe_goal != nullptr ? unsafe_goal->shooter() : nullptr,
+          position->shooter(), status->fbb(),
+          output != nullptr ? &(output_struct) : nullptr, position_timestamp);
+
   climber_.Iterate(unsafe_goal, output != nullptr ? &(output_struct) : nullptr);
 
   bool zeroed;
@@ -81,6 +91,7 @@
   status_builder.add_hood(hood_status_offset);
   status_builder.add_intake(intake_status_offset);
   status_builder.add_turret(turret_status_offset);
+  status_builder.add_shooter(shooter_status_offset);
 
   status->Send(status_builder.Finish());