commit 10b8f4cd448fcd0b59506b1b51537da3307d621d
author Brian Silverman <brians> Sat Mar 21 23:44:46 2015 -0700
committer Brian Silverman <brians> Sun Mar 22 17:08:08 2015 -0400
tree 145b913882af7dfc0d6520f53f48acfd72058151
parent 962808eb459ca15629b3853f6c483349d91ecba4

don't estop the fridge when it shouldn't be

Change-Id: Ic21dceb7c840bccef3c11dcd6f536debf022ac66

frc971/constants.cc

diff --git a/frc971/constants.cc b/frc971/constants.cc
index 0e6fb01..ec98743 100644
--- a/frc971/constants.cc
+++ b/frc971/constants.cc
@@ -60,8 +60,8 @@
 
 const double kArmZeroingHeight = 0.2;
 
-const double kMaxAllowedLeftRightArmDifference = 0.04;       // radians
-const double kMaxAllowedLeftRightElevatorDifference = 0.01;  // meters
+const double kMaxAllowedLeftRightArmDifference = 0.12;  // radians
+const double kMaxAllowedLeftRightElevatorDifference = 0.04;  // meters
 
 const Values::ClawGeometry kClawGeometry{
     // Horizontal distance from the center of the grabber to the end.

frc971/control_loops/fridge/fridge.cc

diff --git a/frc971/control_loops/fridge/fridge.cc b/frc971/control_loops/fridge/fridge.cc
index 0c96b23..48f6cf4 100644
--- a/frc971/control_loops/fridge/fridge.cc
+++ b/frc971/control_loops/fridge/fridge.cc
@@ -514,7 +514,9 @@
         right_arm(), values.max_allowed_left_right_arm_difference);
 
     // Indicate an ESTOP condition and stop the motors.
-    state_ = ESTOP;
+    if (output) {
+      state_ = ESTOP;
+    }
     disable = true;
   }
 
@@ -525,7 +527,9 @@
         values.max_allowed_left_right_elevator_difference);
 
     // Indicate an ESTOP condition and stop the motors.
-    state_ = ESTOP;
+    if (output) {
+      state_ = ESTOP;
+    }
     disable = true;
   }
 
@@ -563,7 +567,9 @@
       LOG(ERROR, "Left arm at %f out of bounds [%f, %f], ESTOPing\n",
           left_arm(), values.fridge.arm.lower_hard_limit,
           values.fridge.arm.upper_hard_limit);
-      state_ = ESTOP;
+      if (output) {
+        state_ = ESTOP;
+      }
     }
 
     if (right_arm() >= values.fridge.arm.upper_hard_limit ||
@@ -571,23 +577,27 @@
       LOG(ERROR, "Right arm at %f out of bounds [%f, %f], ESTOPing\n",
           right_arm(), values.fridge.arm.lower_hard_limit,
           values.fridge.arm.upper_hard_limit);
-      state_ = ESTOP;
+      if (output) {
+        state_ = ESTOP;
+      }
     }
 
-    if (left_elevator() >= values.fridge.elevator.upper_hard_limit ||
-        left_elevator() <= values.fridge.elevator.lower_hard_limit) {
+    if (left_elevator() >= values.fridge.elevator.upper_hard_limit) {
       LOG(ERROR, "Left elevator at %f out of bounds [%f, %f], ESTOPing\n",
           left_elevator(), values.fridge.elevator.lower_hard_limit,
           values.fridge.elevator.upper_hard_limit);
-      state_ = ESTOP;
+      if (output) {
+        state_ = ESTOP;
+      }
     }
 
-    if (right_elevator() >= values.fridge.elevator.upper_hard_limit ||
-        right_elevator() <= values.fridge.elevator.lower_hard_limit) {
+    if (right_elevator() >= values.fridge.elevator.upper_hard_limit) {
       LOG(ERROR, "Right elevator at %f out of bounds [%f, %f], ESTOPing\n",
           right_elevator(), values.fridge.elevator.lower_hard_limit,
           values.fridge.elevator.upper_hard_limit);
-      state_ = ESTOP;
+      if (output) {
+        state_ = ESTOP;
+      }
     }
   }