commit 3f5c1e9ac0729576ca3ba8d9f4a373737faa2586
author Adam Snaider <adsnaider@gmail.com> Fri Mar 24 21:06:03 2017 -0700
committer Austin Schuh <austin.linux@gmail.com> Sun Apr 09 15:55:03 2017 -0700
tree 34d6f01f2a73b3ed099532cf334476e76f5d7ab2
parent 471d3b1acf0582deb08a668575b6019821ed57d2

Hood should not break itself.

Change-Id: Ia099241a354c94b69d4cbaeaa3f7b8ab0758b788

y2017/control_loops/superstructure/hood/hood.h

diff --git a/y2017/control_loops/superstructure/hood/hood.h b/y2017/control_loops/superstructure/hood/hood.h
index 41cbbd0..cf45850 100644
--- a/y2017/control_loops/superstructure/hood/hood.h
+++ b/y2017/control_loops/superstructure/hood/hood.h
@@ -3,6 +3,7 @@
 
 #include "frc971/control_loops/profiled_subsystem.h"
 #include "y2017/control_loops/superstructure/superstructure.q.h"
+#include "y2017/constants.h"
 
 namespace y2017 {
 namespace control_loops {
@@ -32,7 +33,17 @@
 
   // The zeroing and operating voltages.
   static constexpr double kZeroingVoltage = 2.0;
-  static constexpr double kOperatingVoltage = 4.0;
+  static constexpr double kOperatingVoltage = 12.0;
+
+  // Constants needed when determining if the hood is in danger of breaking its
+  // own gears at the normal operating voltage.
+  static constexpr double kErrorOnPositionTillNotMoving =
+      2.5 * (::y2017::constants::Values::kHoodEncoderIndexDifference /
+             ::y2017::constants::Values::kHoodEncoderCountsPerRevolution);
+
+  static constexpr ::aos::monotonic_clock::duration kTimeTillNotMoving =
+      ::std::chrono::milliseconds(15);
+  static constexpr double kNotMovingVoltage = 2.0;
 
   void Iterate(const control_loops::HoodGoal *unsafe_goal,
                const ::frc971::IndexPosition *position, double *output,
@@ -54,6 +65,9 @@
   State state_;
 
   IndexPulseProfiledSubsystem profiled_subsystem_;
+  double last_position_ = 0;
+  ::aos::monotonic_clock::time_point last_move_time_ =
+      ::aos::monotonic_clock::min_time;
 };
 
 }  // namespace hood