commit 36407c908da56b885a210169bbac8107d1ac0352
author Brian Silverman <brians> Tue Apr 01 15:54:02 2014 -0700
committer Brian Silverman <brians> Tue Apr 01 15:54:02 2014 -0700
tree 4e0ce39bc1945384a1d52c410b5172df2bc03068
parent b3adec1a8ba6af5f101303ec9050a0e15a0817ea

fixed the shooter estopping when it looses communications

frc971/control_loops/shooter/shooter.cc

diff --git a/frc971/control_loops/shooter/shooter.cc b/frc971/control_loops/shooter/shooter.cc
index da2aa42..e55c43b 100755
--- a/frc971/control_loops/shooter/shooter.cc
+++ b/frc971/control_loops/shooter/shooter.cc
@@ -10,6 +10,7 @@
 
 #include "frc971/constants.h"
 #include "frc971/control_loops/shooter/shooter_motor_plant.h"
+#include "frc971/queues/output_check.q.h"
 
 namespace frc971 {
 namespace control_loops {
@@ -194,6 +195,18 @@
 
   const bool disabled =
       !::aos::robot_state.get() || !::aos::robot_state->enabled;
+  output_check_received.FetchLatest();
+  // True if we're enabled but the motors aren't working.
+  // TODO(brians): Make this more general.
+  // The 100ms is the result of disabling the robot while it's putting out a lot
+  // of power and looking at the time delay between the last PWM pulse and the
+  // battery voltage coming back up.
+  const bool motors_off =
+      !disabled && (!output_check_received.get() ||
+                    !output_check_received.IsNewerThanMS(100));
+  motors_off_log_.Print();
+  if (motors_off) LOG_INTERVAL(motors_off_log_);
+
   // If true, move the goal if we saturate.
   bool cap_goal = false;
 
@@ -318,6 +331,9 @@
         // useful sensor data.
         latch_piston_ = true;
       }
+      if (motors_off) {
+        load_timeout_ += ::aos::control_loops::kLoopFrequency;
+      }
       // Go to 0, which should be the latch position, or trigger a hall effect
       // on the way.  If we don't see edges where we are supposed to, the
       // offset will be updated by code above.
@@ -625,7 +641,9 @@
     if (cap_goal) {
       shooter_.CapGoal();
     }
-    if (output) output->voltage = shooter_.voltage();
+    // We don't really want to output anything if we went through everything
+    // assuming the motors weren't working.
+    if (output && !motors_off) output->voltage = shooter_.voltage();
   } else {
     shooter_.Update(true);
     shooter_.ZeroPower();

frc971/control_loops/shooter/shooter.gyp

diff --git a/frc971/control_loops/shooter/shooter.gyp b/frc971/control_loops/shooter/shooter.gyp
index 10e0f4e..23380df 100755
--- a/frc971/control_loops/shooter/shooter.gyp
+++ b/frc971/control_loops/shooter/shooter.gyp
@@ -31,6 +31,8 @@
         '<(DEPTH)/frc971/frc971.gyp:constants',
         '<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
         '<(AOS)/common/logging/logging.gyp:queue_logging',
+        '<(AOS)/common/util/util.gyp:log_interval',
+        '<(DEPTH)/frc971/queues/queues.gyp:output_check',
       ],
       'export_dependent_settings': [
         'shooter_loop',

frc971/control_loops/shooter/shooter.h

diff --git a/frc971/control_loops/shooter/shooter.h b/frc971/control_loops/shooter/shooter.h
index 1ab224b..2307c57 100755
--- a/frc971/control_loops/shooter/shooter.h
+++ b/frc971/control_loops/shooter/shooter.h
@@ -6,6 +6,7 @@
 #include "aos/common/control_loop/ControlLoop.h"
 #include "frc971/control_loops/state_feedback_loop.h"
 #include "aos/common/time.h"
+#include "aos/common/util/log_interval.h"
 
 #include "frc971/constants.h"
 #include "frc971/control_loops/shooter/shooter_motor_plant.h"
@@ -207,6 +208,10 @@
   bool last_distal_current_;
   bool last_proximal_current_;
 
+  ::aos::util::SimpleLogInterval motors_off_log_ =
+      ::aos::util::SimpleLogInterval(::aos::time::Time::InSeconds(0.05),
+                                     WARNING, "motors disabled");
+
   DISALLOW_COPY_AND_ASSIGN(ShooterMotor);
 };