commit 34aad4237e301acb4342248a66ed0056948884d8
author Austin Schuh <austin.linux@gmail.com> Sat Mar 28 14:23:31 2015 -0700
committer Michael Schuh <michael@boardsailor.com> Sun Mar 29 14:02:21 2015 -0700
tree a3dc01ebc42b5e3ad0c0575830b8d2f9f34e2273
parent 2fe80a83322821aa6fddb95a8a8575673319530b

Fixed fridge to actually wait until it was high enough when zeroing.

Change-Id: I7f23b504f40c1fc2ff2d7e4ebdfbb7a7b1445972

frc971/control_loops/fridge/fridge.cc

diff --git a/frc971/control_loops/fridge/fridge.cc b/frc971/control_loops/fridge/fridge.cc
index e414f93..14514a9 100644
--- a/frc971/control_loops/fridge/fridge.cc
+++ b/frc971/control_loops/fridge/fridge.cc
@@ -335,12 +335,11 @@
             elevator_goal_ += kElevatorSafeHeightVelocity *
                               ::aos::controls::kLoopFrequency.ToSeconds();
             elevator_goal_velocity_ = kElevatorSafeHeightVelocity;
+            state_ = ZEROING_ELEVATOR;
           } else {
             // We want it stopped at whatever height it's currently set to.
             elevator_goal_velocity_ = 0;
           }
-
-          state_ = ZEROING_ELEVATOR;
           break;
         }
 
@@ -365,7 +364,8 @@
     case ZEROING_ARM:
       LOG(DEBUG, "Zeroing the arm\n");
 
-      if (elevator() < values.fridge.arm_zeroing_height) {
+      if (elevator() < values.fridge.arm_zeroing_height - 0.10 ||
+          elevator_goal_ < values.fridge.arm_zeroing_height) {
         LOG(INFO,
             "Going back to ZEROING_ELEVATOR until it gets high enough to "
             "safely zero the arm\n");