commit 7eecd7c291ebf00b7ad03a1c0e3fed3368778139
author Austin Schuh <austin.linux@gmail.com> Sun Feb 28 21:59:05 2016 -0800
committer Austin Schuh <austin.linux@gmail.com> Mon Feb 29 01:06:02 2016 -0800
tree c510229b14c1c66985bb303c53452ec421840df9
parent 740a09078cbe612a6b8350779f8873bef04a1bce

Added shooter spinup wait state machine.

Change-Id: Ie4ee7dff2706c780b72d10951d63756addc37aa7

y2016/control_loops/shooter/shooter.cc

diff --git a/y2016/control_loops/shooter/shooter.cc b/y2016/control_loops/shooter/shooter.cc
index 0e8f3a5..2f1ae4a 100644
--- a/y2016/control_loops/shooter/shooter.cc
+++ b/y2016/control_loops/shooter/shooter.cc
@@ -11,6 +11,8 @@
 namespace control_loops {
 namespace shooter {
 
+using ::aos::time::Time;
+
 // TODO(austin): Pseudo current limit?
 
 ShooterSide::ShooterSide()
@@ -67,7 +69,8 @@
 }
 
 Shooter::Shooter(ShooterQueue *my_shooter)
-    : aos::controls::ControlLoop<ShooterQueue>(my_shooter) {}
+    : aos::controls::ControlLoop<ShooterQueue>(my_shooter),
+      last_pre_shot_timeout_(0, 0) {}
 
 void Shooter::RunIteration(const ShooterQueue::Goal *goal,
                            const ShooterQueue::Position *position,
@@ -94,8 +97,53 @@
     output->voltage_right = right_.voltage();
 
     if (goal) {
+      bool shoot = false;
+      switch (state_) {
+        case ShooterLatchState::PASS_THROUGH:
+          if (goal->push_to_shooter) {
+            if (::std::abs(goal->angular_velocity) > 10) {
+              if (status->ready) {
+                state_ = ShooterLatchState::WAITING_FOR_SPINDOWN;
+                shoot = true;
+              }
+            } else {
+              shoot = true;
+            }
+          }
+          last_pre_shot_timeout_ = Time::Now() + Time::InSeconds(1.0);
+          break;
+        case ShooterLatchState::WAITING_FOR_SPINDOWN:
+          shoot = true;
+          if (left_.velocity() < goal->angular_velocity * 0.9 ||
+              right_.velocity() < goal->angular_velocity * 0.9) {
+            state_ = ShooterLatchState::WAITING_FOR_SPINUP;
+          }
+          if (::std::abs(goal->angular_velocity) < 10 ||
+              last_pre_shot_timeout_ < Time::Now()) {
+            state_ = ShooterLatchState::WAITING_FOR_SHOT_NEGEDGE;
+          }
+          break;
+        case ShooterLatchState::WAITING_FOR_SPINUP:
+          shoot = true;
+          if (left_.velocity() > goal->angular_velocity * 0.95 &&
+              right_.velocity() > goal->angular_velocity * 0.95) {
+            state_ = ShooterLatchState::WAITING_FOR_SHOT_NEGEDGE;
+          }
+          if (::std::abs(goal->angular_velocity) < 10 ||
+              last_pre_shot_timeout_ < Time::Now()) {
+            state_ = ShooterLatchState::WAITING_FOR_SHOT_NEGEDGE;
+          }
+          break;
+        case ShooterLatchState::WAITING_FOR_SHOT_NEGEDGE:
+          shoot = true;
+          if (!goal->push_to_shooter) {
+            state_ = ShooterLatchState::PASS_THROUGH;
+          }
+          break;
+      }
+
       output->clamp_open = goal->clamp_open;
-      output->push_to_shooter = goal->push_to_shooter;
+      output->push_to_shooter = shoot;
     }
   }
 }