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.h

diff --git a/y2016/control_loops/shooter/shooter.h b/y2016/control_loops/shooter/shooter.h
index 0354bb5..d3a2f86 100644
--- a/y2016/control_loops/shooter/shooter.h
+++ b/y2016/control_loops/shooter/shooter.h
@@ -4,6 +4,7 @@
 #include <memory>
 
 #include "aos/common/controls/control_loop.h"
+#include "aos/common/time.h"
 #include "frc971/control_loops/state_feedback_loop.h"
 
 #include "y2016/control_loops/shooter/shooter_integral_plant.h"
@@ -32,6 +33,9 @@
   // Returns the control loop calculated voltage.
   double voltage() const;
 
+  // Returns the instantaneous velocity.
+  double velocity() const { return loop_->X_hat(1, 0); }
+
   // Executes the control loop for a cycle.
   void Update(bool disabled);
 
@@ -44,7 +48,7 @@
   // History array for calculating a filtered angular velocity.
   static constexpr int kHistoryLength = 10;
   ::std::array<double, kHistoryLength> history_;
-  ptrdiff_t history_position_;
+  ptrdiff_t history_position_ = 0;
 
   DISALLOW_COPY_AND_ASSIGN(ShooterSide);
 };
@@ -54,6 +58,17 @@
   explicit Shooter(
       ShooterQueue *shooter_queue = &control_loops::shooter::shooter_queue);
 
+  enum class ShooterLatchState {
+    // Any shoot commands will be passed through without modification.
+    PASS_THROUGH = 0,
+    // We are latched shooting waiting for the wheel to loose RPM.
+    WAITING_FOR_SPINDOWN = 1,
+    // We are latched shooting waiting for the wheel to spin back up.
+    WAITING_FOR_SPINUP = 2,
+    // Wait until the button is released.
+    WAITING_FOR_SHOT_NEGEDGE = 3
+  };
+
  protected:
   void RunIteration(const ShooterQueue::Goal *goal,
                     const ShooterQueue::Position *position,
@@ -63,6 +78,10 @@
  private:
   ShooterSide left_, right_;
 
+  // Current state.
+  ShooterLatchState state_ = ShooterLatchState::PASS_THROUGH;
+  ::aos::time::Time last_pre_shot_timeout_;
+
   DISALLOW_COPY_AND_ASSIGN(Shooter);
 };