commit 651c4b495a1a4434b9b7facc05a686d0f7e640d2
author Campbell Crowley <mail@campbellcrowley.com> Fri Feb 17 22:30:50 2017 -0800
committer Campbell Crowley <mail@campbellcrowley.com> Sun Feb 19 00:25:15 2017 -0800
tree 43d1b21a9a42b5b979be5cb856f449cb2e75e40b
parent cd3237ac40ec65657c4be2f382b1b469ef5ce597

Add rollers to superstructure

Rebased off master

Change-Id: I905a678aab45cc999526c5076bba86bc8c6597c0

y2017/control_loops/superstructure/superstructure.cc

diff --git a/y2017/control_loops/superstructure/superstructure.cc b/y2017/control_loops/superstructure/superstructure.cc
index 3cad298..0b5f2d9 100644
--- a/y2017/control_loops/superstructure/superstructure.cc
+++ b/y2017/control_loops/superstructure/superstructure.cc
@@ -11,6 +11,12 @@
 namespace control_loops {
 namespace superstructure {
 
+namespace {
+// The maximum voltage the intake roller will be allowed to use.
+constexpr double kMaxIntakeRollerVoltage = 12.0;
+constexpr double kMaxIndexerRollerVoltage = 12.0;
+}  // namespace
+
 Superstructure::Superstructure(
     control_loops::SuperstructureQueue *superstructure_queue)
     : aos::controls::ControlLoop<control_loops::SuperstructureQueue>(
@@ -35,22 +41,33 @@
                 output != nullptr ? &(output->voltage_hood) : nullptr,
                 &(status->hood));
   turret_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->turret) : nullptr,
-                &(position->turret),
-                output != nullptr ? &(output->voltage_turret) : nullptr,
-                &(status->turret));
+                  &(position->turret),
+                  output != nullptr ? &(output->voltage_turret) : nullptr,
+                  &(status->turret));
 
   intake_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->intake) : nullptr,
                   &(position->intake),
                   output != nullptr ? &(output->voltage_intake) : nullptr,
                   &(status->intake));
   shooter_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->shooter) : nullptr,
-                &(position->theta_shooter),
-                output != nullptr ? &(output->voltage_shooter) : nullptr,
-                &(status->shooter));
+                   &(position->theta_shooter),
+                   output != nullptr ? &(output->voltage_shooter) : nullptr,
+                   &(status->shooter));
   indexer_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->indexer) : nullptr,
                 &(position->theta_indexer),
                 output != nullptr ? &(output->voltage_indexer) : nullptr,
                 &(status->indexer));
+
+  if (output && unsafe_goal) {
+    output->voltage_intake_rollers =
+        ::std::max(-kMaxIntakeRollerVoltage,
+                   ::std::min(unsafe_goal->intake.voltage_rollers,
+                              kMaxIntakeRollerVoltage));
+    output->voltage_indexer_rollers =
+        ::std::max(-kMaxIndexerRollerVoltage,
+                   ::std::min(unsafe_goal->indexer.voltage_rollers,
+                              kMaxIndexerRollerVoltage));
+  }
 }
 
 }  // namespace superstructure