commit c59171bf127909a6f7d111f2ecd9a447031212b0
author Sabina Leaver <100027607@mvla.net> Sat Aug 14 14:22:18 2021 -0700
committer Sabina Leaver <100027607@mvla.net> Wed Sep 08 20:32:17 2021 -0700
tree 4abd64e6b73288a0705639872133dcfc4660aaae
parent c5f519d960bb1cfcb55587966d2e621e3b46e5ed

Implementing the 3rd robot superstructure

Signed-off-by: Sabina Leaver <100027607@mvla.net>
Change-Id: Ifae4ab862d15e52d6010de7da5a87d2a0c94e48d
Signed-off-by: Sabina Leaver <100027607@mvla.net>

y2021_bot3/control_loops/superstructure/superstructure.cc

diff --git a/y2021_bot3/control_loops/superstructure/superstructure.cc b/y2021_bot3/control_loops/superstructure/superstructure.cc
index aaf3def..22c7c44 100644
--- a/y2021_bot3/control_loops/superstructure/superstructure.cc
+++ b/y2021_bot3/control_loops/superstructure/superstructure.cc
@@ -16,7 +16,7 @@
   event_loop->SetRuntimeRealtimePriority(30);
 }
 
-void Superstructure::RunIteration(const Goal * /*unsafe_goal*/,
+void Superstructure::RunIteration(const Goal *unsafe_goal,
                                   const Position * /*position*/,
                                   aos::Sender<Output>::Builder *output,
                                   aos::Sender<Status>::Builder *status) {
@@ -24,8 +24,13 @@
     AOS_LOG(ERROR, "WPILib reset, restarting\n");
   }
 
-  if (output != nullptr) {
+  if (output != nullptr && unsafe_goal != nullptr) {
     OutputT output_struct;
+
+    output_struct.intake_volts =
+        std::clamp(unsafe_goal->intake_speed(), -12.0, 12.0);
+    output_struct.outtake_volts =
+        std::clamp(unsafe_goal->outtake_speed(), -12.0, 12.0);
     output->Send(Output::Pack(*output->fbb(), &output_struct));
   }
 
@@ -34,6 +39,11 @@
   status_builder.add_zeroed(true);
   status_builder.add_estopped(false);
 
+  if (unsafe_goal != nullptr) {
+    status_builder.add_intake_speed(unsafe_goal->intake_speed());
+    status_builder.add_outtake_speed(unsafe_goal->outtake_speed());
+  }
+
   status->Send(status_builder.Finish());
 }