commit 7ffb138c872f66ebd2b42f90a055d42d8e4a7a4b
author Brian Silverman <brians> Sun Sep 29 16:55:12 2013 -0700
committer Brian Silverman <brians> Sun Sep 29 16:55:12 2013 -0700
tree 081896dbdb0661529cf5b6cc62fff5c576897089
parent 6da70211b0bac3c3d19d432822b2ebc15f809c10

added a lockout so it won't stop the shooter too soon

frc971/control_loops/shooter/shooter.cc

diff --git a/frc971/control_loops/shooter/shooter.cc b/frc971/control_loops/shooter/shooter.cc
index 981df5d..035880e 100644
--- a/frc971/control_loops/shooter/shooter.cc
+++ b/frc971/control_loops/shooter/shooter.cc
@@ -4,6 +4,7 @@
 #include "aos/common/logging/logging.h"
 
 #include "frc971/control_loops/shooter/shooter_motor_plant.h"
+#include "frc971/control_loops/index/index_motor.q.h"
 
 namespace frc971 {
 namespace control_loops {
@@ -13,7 +14,8 @@
     loop_(new StateFeedbackLoop<2, 1, 1>(MakeShooterLoop())),
     history_position_(0),
     position_goal_(0.0),
-    last_position_(0.0) {
+    last_position_(0.0),
+    last_velocity_goal_(0) {
   memset(history_, 0, sizeof(history_));
 }
 
@@ -26,11 +28,23 @@
     const control_loops::ShooterLoop::Position *position,
     ::aos::control_loops::Output *output,
     control_loops::ShooterLoop::Status *status) {
-  const double velocity_goal = std::min(goal->velocity, kMaxSpeed);
+  double velocity_goal = std::min(goal->velocity, kMaxSpeed);
   const double current_position =
       (position == NULL ? loop_->X_hat(0, 0) : position->position);
   double output_voltage = 0.0;
 
+  if (index_loop.status.FetchLatest() || index_loop.status.get()) {
+    if (index_loop.status->is_shooting) {
+      if (velocity_goal != last_velocity_goal_ &&
+          velocity_goal < 130) {
+        velocity_goal = last_velocity_goal_;
+      }
+    }
+  } else {
+    LOG(WARNING, "assuming index isn't shooting\n");
+  }
+  last_velocity_goal_ = velocity_goal;
+
   // Track the current position if the velocity goal is small.
   if (velocity_goal <= 1.0) {
     position_goal_ = current_position;

frc971/control_loops/shooter/shooter.gyp

diff --git a/frc971/control_loops/shooter/shooter.gyp b/frc971/control_loops/shooter/shooter.gyp
index b1371a4..41c33c1 100644
--- a/frc971/control_loops/shooter/shooter.gyp
+++ b/frc971/control_loops/shooter/shooter.gyp
@@ -29,6 +29,7 @@
         '<(AOS)/common/common.gyp:controls',
         '<(DEPTH)/frc971/frc971.gyp:common',
         '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+        '<(DEPTH)/frc971/control_loops/index/index.gyp:index_loop',
       ],
       'export_dependent_settings': [
         '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',

frc971/control_loops/shooter/shooter.h

diff --git a/frc971/control_loops/shooter/shooter.h b/frc971/control_loops/shooter/shooter.h
index 77c605b..7947f7a 100644
--- a/frc971/control_loops/shooter/shooter.h
+++ b/frc971/control_loops/shooter/shooter.h
@@ -45,6 +45,9 @@
   double position_goal_;
   double last_position_;
 
+  // For making sure it keeps spinning if we're shooting.
+  double last_velocity_goal_;
+
   DISALLOW_COPY_AND_ASSIGN(ShooterMotor);
 };