commit 17dd089d6679e57156fe774c08c30b6ff877dc60
author Austin Schuh <austin.linux@gmail.com> Fri Mar 02 20:06:31 2018 -0800
committer Austin Schuh <austin.linux@gmail.com> Sat Mar 03 00:01:35 2018 -0800
tree 0af5da5e0b79b7f7c08f1392f9ac88695e162695
parent 6829f76c133dc4d25ae61c8bdd02e31c87bc1ff7

Tuned intake.

Change-Id: I028f6cbb8df55d281734ca39abb4f62b7fd27793

y2018/control_loops/superstructure/superstructure.cc

diff --git a/y2018/control_loops/superstructure/superstructure.cc b/y2018/control_loops/superstructure/superstructure.cc
index f53a6db..1c5a4e5 100644
--- a/y2018/control_loops/superstructure/superstructure.cc
+++ b/y2018/control_loops/superstructure/superstructure.cc
@@ -63,12 +63,66 @@
                    status->arm.zeroed;
 
   if (output && unsafe_goal) {
-    output->intake.left.voltage_rollers = ::std::max(
+    const double roller_voltage = ::std::max(
         -kMaxIntakeRollerVoltage, ::std::min(unsafe_goal->intake.roller_voltage,
                                              kMaxIntakeRollerVoltage));
-    output->intake.right.voltage_rollers = ::std::max(
-        -kMaxIntakeRollerVoltage, ::std::min(unsafe_goal->intake.roller_voltage,
-                                             kMaxIntakeRollerVoltage));
+    constexpr int kReverseTime = 15;
+    if (unsafe_goal->intake.roller_voltage < 0.0) {
+      output->intake.left.voltage_rollers = roller_voltage;
+      output->intake.right.voltage_rollers = roller_voltage;
+      rotation_state_ = RotationState::NOT_ROTATING;
+      rotation_count_ = 0;
+    } else {
+      switch (rotation_state_) {
+        case RotationState::NOT_ROTATING:
+          if (position->intake.left.beam_break) {
+            rotation_state_ = RotationState::ROTATING_RIGHT;
+            rotation_count_ = kReverseTime;
+            break;
+          } else if (position->intake.right.beam_break) {
+            rotation_state_ = RotationState::ROTATING_LEFT;
+            rotation_count_ = kReverseTime;
+            break;
+          } else {
+            break;
+          }
+        case RotationState::ROTATING_LEFT:
+          if (position->intake.right.beam_break) {
+            rotation_count_ = kReverseTime;
+          } else {
+            --rotation_count_;
+          }
+          if (rotation_count_ == 0) {
+            rotation_state_ = RotationState::NOT_ROTATING;
+          }
+          break;
+        case RotationState::ROTATING_RIGHT:
+          if (position->intake.left.beam_break) {
+            rotation_count_ = kReverseTime;
+          } else {
+            --rotation_count_;
+          }
+          if (rotation_count_ == 0) {
+            rotation_state_ = RotationState::NOT_ROTATING;
+          }
+          break;
+      }
+
+      switch (rotation_state_) {
+        case RotationState::NOT_ROTATING:
+          output->intake.left.voltage_rollers = roller_voltage;
+          output->intake.right.voltage_rollers = roller_voltage;
+          break;
+        case RotationState::ROTATING_LEFT:
+          output->intake.left.voltage_rollers = roller_voltage;
+          output->intake.right.voltage_rollers = -roller_voltage;
+          break;
+        case RotationState::ROTATING_RIGHT:
+          output->intake.left.voltage_rollers = -roller_voltage;
+          output->intake.right.voltage_rollers = roller_voltage;
+          break;
+      }
+    }
   }
 }