commit ed9df2fd9d8768313fb0ce4dd4f5732295c748bc
author Brian Silverman <brians> Sat Apr 05 07:07:15 2014 -0700
committer Brian Silverman <brians> Sat Apr 05 07:07:15 2014 -0700
tree 68be61e16b84ac68cc7aeaad5bca3596619722b4
parent fba7163b6bf0acf745dd12d1ecbb167fd8b59b71

fixed claw power capping before it knows where they are

frc971/control_loops/claw/claw.cc

diff --git a/frc971/control_loops/claw/claw.cc b/frc971/control_loops/claw/claw.cc
index 611dd0e..f9ee982 100644
--- a/frc971/control_loops/claw/claw.cc
+++ b/frc971/control_loops/claw/claw.cc
@@ -191,21 +191,25 @@
 
     {
       const auto values = constants::GetValues().claw;
-      if (X_hat(0, 0) + X_hat(1, 0) > values.upper_claw.upper_limit &&
-          U(1, 0) > 0) {
-        LOG(WARNING, "upper claw too high and moving up\n");
-        U(1, 0) = 0;
-      } else if (X_hat(0, 0) + X_hat(1, 0) < values.upper_claw.lower_limit &&
-                 U(1, 0) < 0) {
-        LOG(WARNING, "upper claw too low and moving down\n");
-        U(1, 0) = 0;
+      if (top_known_) {
+        if (X_hat(0, 0) + X_hat(1, 0) > values.upper_claw.upper_limit &&
+            U(1, 0) > 0) {
+          LOG(WARNING, "upper claw too high and moving up\n");
+          U(1, 0) = 0;
+        } else if (X_hat(0, 0) + X_hat(1, 0) < values.upper_claw.lower_limit &&
+                   U(1, 0) < 0) {
+          LOG(WARNING, "upper claw too low and moving down\n");
+          U(1, 0) = 0;
+        }
       }
-      if (X_hat(0, 0) > values.lower_claw.upper_limit && U(0, 0) > 0) {
-        LOG(WARNING, "lower claw too high and moving up\n");
-        U(0, 0) = 0;
-      } else if (X_hat(0, 0) < values.lower_claw.lower_limit && U(0, 0) < 0) {
-        LOG(WARNING, "lower claw too low and moving down\n");
-        U(0, 0) = 0;
+      if (bottom_known_) {
+        if (X_hat(0, 0) > values.lower_claw.upper_limit && U(0, 0) > 0) {
+          LOG(WARNING, "lower claw too high and moving up\n");
+          U(0, 0) = 0;
+        } else if (X_hat(0, 0) < values.lower_claw.lower_limit && U(0, 0) < 0) {
+          LOG(WARNING, "lower claw too low and moving down\n");
+          U(0, 0) = 0;
+        }
       }
     }
   }
@@ -806,6 +810,7 @@
     LimitClawGoal(&bottom_claw_goal_, &top_claw_goal_, values);
   }
 
+  claw_.set_positions_known(top_claw_.zeroing_state() != ZeroedStateFeedbackLoop::UNKNOWN_POSITION, bottom_claw_.zeroing_state() != ZeroedStateFeedbackLoop::UNKNOWN_POSITION);
   if (has_top_claw_goal_ && has_bottom_claw_goal_) {
     claw_.R << bottom_claw_goal_, top_claw_goal_ - bottom_claw_goal_,
         bottom_claw_velocity_, top_claw_velocity_ - bottom_claw_velocity_;

frc971/control_loops/claw/claw.h

diff --git a/frc971/control_loops/claw/claw.h b/frc971/control_loops/claw/claw.h
index 6dbc408..8c81e62 100644
--- a/frc971/control_loops/claw/claw.h
+++ b/frc971/control_loops/claw/claw.h
@@ -29,6 +29,10 @@
   virtual void CapU();
 
   void set_is_zeroing(bool is_zeroing) { is_zeroing_ = is_zeroing; }
+  void set_positions_known(bool top_known, bool bottom_known) {
+    top_known_ = top_known;
+    bottom_known_ = bottom_known;
+  }
 
   void ChangeTopOffset(double doffset);
   void ChangeBottomOffset(double doffset);
@@ -39,6 +43,8 @@
   double uncapped_average_voltage_;
   bool is_zeroing_;
 
+  bool top_known_ = false, bottom_known_ = false;
+
   const ::aos::controls::HPolytope<2> U_Poly_, U_Poly_zeroing_;
 };